DECENTRALIZED  SINGLE-BEACON  ACOUSTIC  NAVIGATION: 
COMBINED  COMMUNICATION  AND  NAVIGATION 
FOR  UNDERWATER  VEHICLES 

by 

Sarah  E.  Webster 

A  dissertation  submitted  to  The  Johns  Hopkins  University  in  conformity  with  the 
requirements  for  the  degree  of  Doctor  of  Philosophy. 

Baltimore,  Maryland 
June,  2010 


©  Sarah  E.  Webster  2010 


All  rights  reserved 


Report  Documentation  Page 


Form  Approved 
OMB  No.  0704-0188 


Public  reporting  burden  for  the  collection  of  information  is  estimated  to  average  1  hour  per  response,  including  the  time  for  reviewing  instructions,  searching  existing  data  sources,  gathering  and 
maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of  information.  Send  comments  regarding  this  burden  estimate  or  any  other  aspect  of  this  collection  of  information, 
including  suggestions  for  reducing  this  burden,  to  Washington  Headquarters  Services,  Directorate  for  Information  Operations  and  Reports,  1215  Jefferson  Davis  Highway,  Suite  1204,  Arlington 
VA  22202-4302.  Respondents  should  be  aware  that  notwithstanding  any  other  provision  of  law,  no  person  shall  be  subject  to  a  penalty  for  failing  to  comply  with  a  collection  of  information  if  it 
does  not  display  a  currently  valid  OMB  control  number. 


1.  REPORT  DATE 

JUN  2010 


2.  REPORT  TYPE 


3.  DATES  COVERED 

00-00-2010  to  00-00-2010 


4.  TITLE  AND  SUBTITLE 


Decentralized  Single-beacon  Acoustic  Navigation:  Combined 
Communication  and  Navigation  for  Underwater  Vehicles 

6.  AUTHOR(S) 


5a.  CONTRACT  NUMBER 


5b.  GRANT  NUMBER 


5c.  PROGRAM  ELEMENT  NUMBER 


5d.  PROJECT  NUMBER 


5e.  TASK  NUMBER 
5f.  WORK  UNIT  NUMBER 

7.  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESS(ES)  8.  PERFORMING  ORGANIZATION 

Johns  Hopkins  University, Baltimore, MD, 21218  report  number 

9.  SPONSORING/MONITORING  AGENCY  NAME(S )  AND  ADDRESS(ES )  10.  SPONSOR/MONITOR' S  ACRONYM(S) 

11.  SPONSOR/MONITOR'S  REPORT 
NUMBER(S) 

12.  DISTRIBUTION/AVAILABILITY  STATEMENT 

Approved  for  public  release;  distribution  unlimited 

13.  SUPPLEMENTARY  NOTES 

14.  ABSTRACT 

This  thesis  reports  the  derivation  and  validation  of  two  single-beacon  acoustic  navigation  algorithms,  as 
well  as  the  development  and  experimental  evaluation  of  a  platformindependent  acoustic  communication 
(Acomms)  system  that  enables  combined  communication  and  navigation.  The  navigation  algorithms  are 
centralized  and  decentralized  formulations  of  single-beacon  navigation,  and  employ  range  measurements 
from  a  single  reference  beacon  to  an  underwater  vehicle  in  addition  to  the  vehicles  Doppler  velocity  log 
gyrocompass,  and  depth  sensors  to  perform  absolute  (as  opposed  to  relative)  localization  and  navigation  of 
the  vehicle.  The  centralized  single-beacon  algorithm  is  based  on  the  extended  Kalman  filter.  We  assume 
that  the  Kalman  filter  has  simultaneous,  real-time  access  to  sensor  measurements  from  both  the  vehicle 
and  the  beacon  (e.g.  the  ship).  The  decentralized  single-beacon  algorithm  is  based  on  the  information  form 
of  the  extended  Kalman  filter.  We  assume  that  the  information  filter  on  the  vehicle  only  has  access  to 
measurements  from  the  vehicles  on-board  navigation  sensors  in  real-time.  The  vehicle-based  filter  receives 
acoustic  broadcasts  from  the  reference  beacon  that  contain  information  about  the  beacons  position  and 
sensor  measurements.  We  show  analytically  and  in  simulation  that  the  decentralized  algorithm  formulated 
herein  yields  an  identical  state  estimate  to  the  state  estimate  of  the  centralized  algorithm  at  the  instant  of 
each  range  measurement;  in  addition  we  show  that  between  range  measurements  the  results  from  the  two 
algorithms  differ  only  by  linearization  errors  and  the  effects  of  smoothing  historic  ship  states.  The  Acomms 
system  has  been  installed  on  theWood  Hole  Oceanographic  Institution  vehicles  Puma,  Jaguar  and  Nereus. 
The  author  and  collaborators  deployed  the  Acomms  system  in  four  sea  trials  in  the  North  Pacific  and 
South  Atlantic  Oceans,  including  the  Mariana  Trench.  The  performance  of  the  navigation  algorithms  are 
evaluated  using  simulations  and  navigation  data  from  these  field  trials.  The  benefit  of  single-beacon 
navigation  and  this  implementation  are  that  the  decentralized  formulation  scales  naturally  to  multiple 
vehicles  and  the  use  of  a  single,  moving  reference  beacon  eliminates  the  need  for  multiple,  fixed  beacons 
and  their  associated  cost  and  range  limitations. 


15.  SUBJECT  TERMS 


16.  SECURITY  CLASSIFICATION  OF: 

17.  LIMITATION  OF 

18.  NUMBER 

1 9a.  NAME  OF 

ABSTRACT 

OF  PAGES 

RESPONSIBLE  PERSON 

a.  REPORT 

unclassified 

b.  ABSTRACT 

unclassified 

c.  THIS  PAGE 

unclassified 

Same  as 
Report  (SAR) 

245 

Standard  Form  298  (Rev.  8-98) 

Prescribed  by  ANSI  Std  Z39-18 


Abstract 


This  thesis  reports  the  derivation  and  validation  of  two  single -beacon  acoustic  navi¬ 
gation  algorithms,  as  well  as  the  development  and  experimental  evaluation  of  a  platform- 
independent  acoustic  communication  (Acomms)  system  that  enables  combined  communi¬ 
cation  and  navigation.  The  navigation  algorithms  are  centralized  and  decentralized  for¬ 
mulations  of  single-beacon  navigation,  and  employ  range  measurements  from  a  single  ref¬ 
erence  beacon  to  an  underwater  vehicle  in  addition  to  the  vehicles  Doppler  velocity  log, 
gyrocompass,  and  depth  sensors  to  perform  absolute  (as  opposed  to  relative)  localization 
and  navigation  of  the  vehicle.  The  centralized  single-beacon  algorithm  is  based  on  the  ex¬ 
tended  Kalman  filter.  We  assume  that  the  Kalman  filter  has  simultaneous,  real-time  access 
to  sensor  measurements  from  both  the  vehicle  and  the  beacon  (e.g.  the  ship).  The  decen¬ 
tralized  single-beacon  algorithm  is  based  on  the  information  form  of  the  extended  Kalman 
filter.  We  assume  that  the  information  filter  on  the  vehicle  only  has  access  to  measure¬ 
ments  from  the  vehicles  on-board  navigation  sensors  in  real-time.  The  vehicle-based  filter 
receives  acoustic  broadcasts  from  the  reference  beacon  that  contain  information  about  the 
beacons  position  and  sensor  measurements.  We  show  analytically  and  in  simulation  that 
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the  decentralized  algorithm  formulated  herein  yields  an  identical  state  estimate  to  the  state 
estimate  of  the  centralized  algorithm  at  the  instant  of  each  range  measurement;  in  addition 
we  show  that  between  range  measurements  the  results  from  the  two  algorithms  differ  only 
by  linearization  errors  and  the  effects  of  smoothing  historic  ship  states.  The  Acomms  sys¬ 
tem  has  been  installed  on  the  Wood  Hole  Oceanographic  Institution  vehicles  Puma ,  Jaguar, 
and  Nereus.  The  author  and  collaborators  deployed  the  Acomms  system  in  four  sea  trials 
in  the  North  Pacific  and  South  Atlantic  Oceans,  including  the  Mariana  Trench.  The  per¬ 
formance  of  the  navigation  algorithms  are  evaluated  using  simulations  and  navigation  data 
from  these  field  trials.  The  benefit  of  single-beacon  navigation  and  this  implementation 
are  that  the  decentralized  formulation  scales  naturally  to  multiple  vehicles  and  the  use  of  a 
single,  moving  reference  beacon  eliminates  the  need  for  multiple,  fixed  beacons  and  their 
associated  cost  and  range  limitations. 
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Chapter  1 


Introduction 


A  great  help  also  would  be  for  the  furtherance  of  skill,  if  those  that  are  practisers  in  that  Arte 
[of  Navigation],  and  such  as  are  Students  of  the  Mathematikes,  might  often  conferre  together. 
For  except  there  be  a  uniting  of  knowledge  with  practice,  there  can  be  nothing  excellent. 


William  Barlow  in  Navigator’s  Supply  (1597) 


1.1  Motivation 

The  world’s  oceans  cover  more  than  70%  of  our  planet,  but  we  presently  know  less 
about  the  submerged  surface  of  the  earth  than  we  do  about  the  surface  of  the  moon.  The 
oceans  hold  information  that  is  important  in  basic  research  and  our  daily  lives.  They  can 
provide  clues  to  widely  disparate  areas  of  study,  such  as  how  species  adapt  to  different 
depths  and  temperatures  and  how  ocean  circulation  contributes  to  global  climate  regula¬ 
tion.  To  help  understand  these  phenomena,  we  use  underwater  robotic  vehicles  to  collect 
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biological  and  geological  samples,  create  accurate  maps,  and  deploy  and  recover  experi¬ 
ments.  To  enable  these  scientific  activities,  we  must  be  able  to  accurately  and  repeatably 
measure  vehicle  position.  The  challenge,  however,  is  that  the  global  positioning  system 
(GPS)  does  not  work  underwater. 

In  lieu  of  GPS,  underwater  navigation  is  most  commonly  accomplished  using  sound — 
for  instance,  garnering  range  measurements  from  the  time-of-flight  of  acoustic  signals. 
However,  existing  high-precision  absolute  acoustic  navigation  methods  for  underwater  ve¬ 
hicles  are  impractical  over  long  length  scales  (on  the  order  of  10  km  or  more).  Moreover, 
many  existing  acoustic  navigation  systems  lack  scalability  for  simultaneously  navigating 
multiple  vehicles.  In  comparison  to  land-based  radio  frequency  navigation  and  commu¬ 
nication  methods,  underwater  acoustic  navigation  and  communication  using  underwater 
modems  suffers  from  severely  limited  bandwidth  and  high  latency  [53].  Given  the  speed  of 
sound  in  water  (~  1 500  m/s),  transmitting  acoustic  data  over  length  scales  on  the  order  of 
kilometers  results  in  latency  on  the  order  of  seconds.  Although  the  bandwidth  of  acoustic 
modem  technology  has  increased  dramatically  in  recent  years,  achieving  throughput  of  up 
to  2400  bps  [84],  operationally  the  average  throughput  is  on  the  order  of  10-50  bps  due  to 
the  low  duty  cycle  with  which  acoustic  messages  are  typically  transmitted  during  under¬ 
water  vehicle  operations.  As  a  result,  existing  multi- vehicle  navigation  algorithms  that  rely 
on  high-bandwidth  communication  are  of  limited  practical  use  underwater. 

The  goal  of  this  thesis  is  to  enable  high-precision  absolute  navigation  of  multiple  under¬ 
water  vehicles  over  length  scales  on  the  order  of  1  to  100  km.  To  support  the  simultaneous 
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navigation  of  multiple  underwater  vehicles,  we  seek  a  flexible,  scalable  solution  through 
a  decentralized  approach.  This  thesis  focuses  on  a  new  navigation  algorithm  employing  a 
single  beacon  that  can  be  mounted  on  a  ship.  The  algorithm  is  designed  to  work  in  a  low- 
bandwidth  environment,  scale  naturally  to  multiple  vehicles,  and  avoid  the  geographical 
constraint  imposed  by  fixed  acoustic  beacons. 

1.2  Thesis  Contributions  and  Roadmap 

This  thesis  reports  the  development,  implementation,  and  evaluation  of  a  new  naviga¬ 
tion  system  for  underwater  vehicles,  as  well  as  the  derivation  of  two  navigation  algorithms 
based  on  ranges  from  a  single  moving  reference  beacon  (e.g.  range-aided  navigation).  The 
navigation  algorithms  are  a  centralized  implementation  and  a  decentralized  implementation 
of  range-aided  navigation  based  on  the  extended  Kalman  filter  and  the  extended  informa¬ 
tion  filter  respectively.  The  navigation  system  was  tested  in  simulation  and  full-scale  field 
trials.  The  centralized  filter  is  experimentally  validated  with  data  collected  during  field 
trials  and  the  decentralized  filter  is  validated  through  simulation. 

Review  of  Underwater  Navigation  and  Previous  Research 

Chapter  2  reviews  the  current  standard  methods  for  underwater  navigation,  the  pre¬ 
vious  research  on  the  relatively  unexploited  method  of  single -beacon  navigation,  and  the 
details  of  single -beacon  one-way-travel-time  (OWTT)  navigation — the  navigation  method 
addressed  in  this  thesis — which  was  first  published  in  [27]  and  [28]. 
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Acoustic  Communications  (Acomms)  System 

Chapter  3  reports  the  development  of  a  novel  navigation  system  for  underwater  vehicles 
that  implements  OWTT  navigation,  employing  synchronous  clocks  and  acoustic  modems 
in  addition  to  standard  shipboard  and  vehicle-based  navigation  sensors  to  achieve  simul¬ 
taneous  acoustic  communication  and  navigation.  Details  of  the  Acomms  system’s  perfor¬ 
mance  and  engineering  accomplishments  during  four  different  oceanographic  expeditions 
are  presented.  This  work  was  published  in  part  by  the  author  and  collaborators  in  [92]. 

System  Models 

Chapter  4  describes  the  process  models  for  the  vehicle  and  the  ship  and  the  observation 
models  used  in  this  thesis.  These  models  are  used  in  the  formulation  of  both  the  Kalman 
filter  and  the  information  filter.  These  models  are  published  in  conjunction  with  the  work 
in  Chapter  5. 

Centralized  Extended  Kalman  Filter  Algorithm  and  Benchmark 

Chapter  5  reports  the  formulation  and  implementation  of  a  centralized  algorithm  based 
on  the  extended  Kalman  filter  and  designed  to  estimate  vehicle  and  ship  position  within 
the  framework  of  single-beacon  OWTT  navigation.  The  centralized  (CEKF)  algorithm  is 
designed  to  have  simultaneous  access  to  all  sensor  measurements  from  both  the  vehicle 
and  the  ship.  Field  results  are  reported  from  an  autonomous  underwater  vehicle  (AUV) 
survey  carried  out  in  4000  m  of  water  on  the  southern  Mid-Atlantic  Ridge  while  carry¬ 
ing  out  a  near-bottom  survey  in  search  of  hydrothermal  vents.  The  experimental  results 
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presented  demonstrate  that  single-beacon  navigation  is  a  viable  alternative  to  traditional 
absolute  acoustic  navigation  methods,  and  the  CEKF  is  used  as  a  benchmark  for  the  decen¬ 
tralized  algorithm  discussed  in  Chapter  6.  This  CEKF  work  was  published  in  part  by  the 
author  and  collaborators  in  [93]. 

Decentralized  Extended  Information  Filter  Algorithm 

Chapter  6  reports  the  derivation  of  a  decentralized  algorithm  based  on  the  extended 
information  filter  for  single-beacon  OWTT  navigation.  The  decentralized  (DEIF)  algorithm 
is  designed  to  run  locally  on  a  submerged  vehicle  with  real-time  access  to  measurements 
from  only  the  vehicle’s  on-board  navigation  sensors  and  with  infrequent,  asynchronous 
reception  of  acoustic  broadcasts  containing  information  from  the  ship.  The  DEIF  algorithm 
is  designed  to  be  implemented  in  real  time  on  one  or  more  underwater  vehicles.  The  DEIF 
is  shown  analytically  to  replicate  exactly  the  state  estimate  of  the  CEKF  immediately  after 
each  range  measurement,  and  the  analytical  results  are  supported  using  a  simulation  of  the 
deep-water  experimental  data  set  from  Chapter  5.  To  the  best  knowledge  of  the  author, 
this  work  is  the  first  to  formulate  and  test  an  extended  information  filter  in  the  context 
of  decentralized  single-beacon  navigation  for  underwater  vehicles.  This  work  has  been 
accepted  for  publication  in  [94]. 

Future  Work 

Conclusions  and  future  work  are  discussed  in  Chapter  7,  including  the  extension  of  the 
decentralized  algorithm  to  multi- vehicle  operations  employing  inter- vehicle  ranges. 
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Underwater  Vehicle  Navigation 


The  ability  to  estimate  the  position  of  an  underwater  vehicle  reliably,  repeatably,  and 
accurately  is  a  necessary  prerequisite  for  collecting  high  resolution  oceanographic  data, 
creating  meaningful,  high-resolution  maps,  and  locating  and  relocating  sites  of  interest  in 
the  ocean.  This  chapter  reviews  conventional  underwater  navigation  methods  as  well  as 
previous  work  reported  in  single-beacon  and  decentralized  navigation.  The  remainder  of 
the  chapter  is  organized  as  follows:  Section  2.1  reviews  conventional  navigation.  Section 
2.2  reviews  previously  reported  results  on  single -beacon  navigation  and  details  of  the  im¬ 
plementation  used  in  this  thesis.  Section  2.3  reviews  previously  reported  results  in  decen¬ 
tralized  estimation  in  the  context  of  underwater  navigation.  Section  2.4  reviews  previously 
reported  navigation  methods,  both  land-  and  water-based,  that  rely  on  the  information  filter. 
Section  2.5  concludes  with  a  brief  literature  review  of  range-only  simultaneous  localization 
and  mapping  (RO-SLAM). 
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2.1  Conventional  Navigation  Methods 

Conventional  underwater  navigation  methods  can  be  loosely  categorized  as  either  meth¬ 
ods  that  provide  bounded-error  position  estimates  or  methods  that  do  not,  resulting  in  po¬ 
sition  estimates  with  errors  that  are  unbounded  over  time. 


2.1.1  Dead  Reckoning 

Navigation  based  on  velocity  and  heading,  commonly  referred  to  as  dead  reckoning, 
has  long  been  a  staple  of  nautical  navigation  [11].  Dead  reckoning  relies  on  an  initial  geo- 
referenced  position  estimate,  referred  to  as  a  position  fix,  and  propagates  this  fix  forward  by 
estimating  the  direction  and  distance  traveled  by  the  ship  or  vehicle.  The  distance  traveled 
is  estimated  by  integrating  velocity  or  acceleration  measurements  to  calculate  relative  dis¬ 
placement.  In  addition  to  these  sensors,  underwater  vehicles  typically  have  a  depth  sensor 
and  a  compass.  The  typical  suite  of  vehicle-based  navigation  sensors  available  to  under¬ 
water  vehicles,  depending  on  the  vehicle  size  and  budget,  is  shown  in  Table  2.1,  where  z 
denotes  depth  or  altitude,  x  denotes  linear  accelerations  in  the  world  frame,  Xi)0(]y  denotes 
linear  accelerations  in  the  body  frame,  u  denotes  angular  velocities,  and  u  denotes  angular 
accelerations. 

In  underwater  vehicle  navigation  several  methods  for  estimating  vehicle  velocity  are 
common.  A  Doppler  velocity  log  currently  provides  the  most  accurate  estimate  of  linear 
velocity  available  to  underwater  vehicles.  On  torpedo- shaped  vehicles  the  angular  velocity 
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Table  2.1:  Navigation  sensors  commonly  used  in  underwater  vehicles. 


Instrument 

Variable 

Update  Rate 

Precision 

Range 

Drift 

Acoustic  Altimeter 

z  (altitude) 

0.1-10  Hz 

0.01-1.0  ill 

varies  w/  freq 

— 

Pressure  Sensor 

2 (depth) 

1  Hz 

0.01-0.1% 

11  km 

— 

Inclinometer 

roll,  pitch 

1-10  Hz 

0.1°-1° 

±45° 

— 

Magnetic  Compass 

heading 

1-10  Hz 

1-10° 

360° 

— 

Gyro:  Mechanical 

heading 

1-10  Hz 

0.1° 

360° 

10° /h 

Gyro:  Ring-laser 
and  Fiber-optic 

heading 

1-1600  Hz 

0.01°-0.1° 

360° 

0.1-10°/ h 

Gyro:  North  Seeking 

heading, 

pitch, 

roll,x,u; 

1-100  Hz 

o 

b 

I—1 

o 

1 

p 

I—1 

o 

360° 

IMU 

X,iO,iU 

1-1000  Hz 

0.01  m 

varies 

varies 

Bottom-lock  Doppler 

%body 

1-5  Hz 

<  0.3% 

18-100 m 

— 

Source:  Kinsey  et  at.  [54] 


of  the  propeller  is  sometimes  used  as  a  proxy  for  a  forward  velocity,  and  the  direction  of 
travel  is  assumed  to  be  parallel  to  heading  of  the  vehicle. 

The  advantage  of  dead  reckoning  is  that  it  can  be  performed  using  only  vehicle-based 
sensors.  The  disadvantage  is  that  dead  reckoning  accrues  error  at  a  rate  determined  by  the 
precision  of  these  sensors  and  yields  an  estimate  of  local  displacement  with  errors  that  are 
unbounded  over  time.  In  order  to  achieve  bounded-error  navigation,  additional  navigation 
information  is  required  from  an  absolute  georeferenced  source. 


2.1.2  Bounded-Error  Navigation 


Bounded-error  navigation  underwater  is  usually  achieved  with  the  aid  of  systems  that 
rely  on  external  sensors  such  as  long  baseline  (LBL)  or  ultra-short  baseline  (USBL)  nav¬ 
igation  [68].  Table  2.2  shows  a  comparison  between  LBL,  USBL,  and  global  positioning 
system  (GPS)  position  measurements.  Between  position  fixes,  a  vehicle  estimates  its  po- 
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sition  using  velocity,  acceleration,  and  attitude  measurements  as  in  dead  reckoning.  An 
extreme  example  of  this  is  underwater  gliders  such  as  the  Slocum  glider  that  uses  GPS  to 
measure  a  position  fix  while  on  the  surface,  but  navigates  solely  by  dead  reckoning  between 
GPS  fixes — typically  for  periods  on  the  order  of  hours  or  days  [91]. 


Table  2.2:  Georeferenced  navigation  sensors  used  to  measure  XYZ  position. 


Instrument 

Variable 

Update  Rate 

Precision 

Range 

Global  Positioning  System 

xyz 

1-10  Hz 

0.1-10  m  in  air 

0  km  in  water 

Long  Baseline  (300  kHz) 

xyz 

1-10  Hz 

±0.007  m 

0.1  km 

Long  Baseline  (12  kHz) 

xyz 

0. 1-1.0  Hz 

0.1-10  m 

5-10  km 

Ultra-Short  Baseline 

range  & 
bearing 

0.02-0.5  Hz 

0. 1-2.0% 
of  range 

0.1-10  km 

Source:  Kinsey  et  al.  [54] 


LBL  navigation  relies  on  acoustic  beacons  that  are  typically  deployed  as  subsurface 
moorings,  50-600  m  above  the  seafloor,  prior  to  vehicle  deployment  [68].  After  deploy¬ 
ment,  the  beacon  locations  are  surveyed  from  the  ship  by  collecting  two-way  travel  times 
between  the  beacon  and  the  ship’s  transducer  as  the  ship  navigates  around  the  estimated 
beacon  location.  The  position  of  the  ship’s  transducer  is  known  from  GPS  and  the  ship’s 
gyrocompass  so  that,  taking  into  account  the  sound  velocity  profile,  a  least  squares  esti¬ 
mate  of  the  beacon  position  can  be  found.  A  careful  beacon  survey  can  result  in  residuals 
on  the  order  of  one  meter  or  less  in  several  thousand  meters  of  water.  Once  the  vehicle  is 
deployed,  it  interrogates  the  beacons  and  the  beacons  respond  with  a  fixed,  known  turn¬ 
around  time.  Using  an  estimate  of  sound  velocity,  the  vehicle  uses  the  two-way  travel-time 
to  estimate  its  range  from  each  beacon  and  can  thus  triangulate  its  own  position. 

The  principal  advantage  of  LBL  navigation  is  that  it  provides  absolute  geodetic  naviga- 
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tion  fixes.  One  of  the  disadvantages  of  LBL  navigation  is  that,  in  most  cases,  the  beacons 
are  fixed,  thus  limiting  the  vehicle  to  a  5-10  km  range  for  12  kHz  LBL.  In  addition,  the 
beacons  and  the  time  required  to  deploy,  survey,  and  recover  them  are  costly  in  terms  of 
the  hardware,  the  cost  of  ship  time,  and  the  time  lost  during  the  expedition.1 

USBL  navigation  relies  on  the  calculation  of  a  vehicle’s  relative  displacement  from  a 
single  external  beacon  typically  mounted  on  a  ship.  Range,  azimuth,  and  elevation  are 
measured  using  an  acoustic  signal — the  two-way  travel  time  of  the  signal  is  used  to  de¬ 
termine  range,  a  small  transducer  array  is  used  to  determine  the  azimuth  and  elevation  of 
the  incoming  acoustic  signal.  Provided  that  the  external  beacon  has  a  known  location,  it  is 
possible  to  compute  a  geodetic  fix  for  the  vehicle.  However,  the  vehicle  position  informa¬ 
tion  is  typically  calculated  at  the  external  beacon,  not  on-board  the  vehicle,  and  is  therefore 
not  available  to  the  vehicle  unless  the  information  is  communicated  acoustically  or  through 
some  other  means. 

Because  USBL  navigation  does  not  rely  on  a  fixed  external  beacon,  as  is  commonly  the 
case  with  LBL  navigation,  the  use  of  USBL  does  not  constrain  the  vehicle  to  an  area  of 
the  seafloor.  However,  the  range  limitations  in  a  USBL  system  are  more  severe  than  LBL 
because  a  USBL  system  measures  range  and  bearing,  and  error  in  the  bearing  measurement 
results  in  greater  error  in  the  position  estimate  of  the  vehicle  at  longer  ranges.  Thus  the 
precision  of  the  vehicle  position  estimate  is  a  function  of  the  range  between  the  ship  and  the 

vehicle.  This  effectively  depth-limits  USBL  systems  depending  on  the  desired  precision. 

'Acoustic  navigation  beacons  rated  for  full-ocean-depth  operation  typically  cost  more  than  $10k  each. 
An  acoustic  beacon  survey  at  full-ocean-depth  takes  ~l-24  hours  of  ship  time,  and  a  large  oceanographic 
ship  such  as  an  AGOR-25  class  vessel,  e.g.  R/V  Atlantis  and  R/V  Thomas  G.  Thompson,  costs  ~$30k/day. 


10 


CHAPTER  2.  UNDERWATER  VEHICLE  NAVIGATION 


2.2  Single-Beacon  Navigation 

In  this  thesis  range-aided  navigation  refers  to  vehicle  navigation  that  relies  on  ranges 
from  one  or  more  georeferenced  sources  in  addition  to  supplementary  non-georeferenced 
navigation  information  from  additional  sources  such  as  inertial  sensors.  The  term  range- 
only  navigation  is  reserved  for  navigation  based  solely  on  range  measurements,  possibly 
from  multiple  sources,  with  no  additional  navigation  information.  Single-beacon  navi¬ 
gation  is  the  minimal  implementation  of  range-aided  navigation,  relying  on  range  mea¬ 
surements  from  a  single,  georeferenced  beacon  to  provide  an  absolute  position  reference. 
Conversely,  long  baseline  could  be  considered  the  maximal  implementation  of  range-aided 
navigation  whereby  ranges  from  multiple  beacons  are  measured  concurrently. 

Compared  to  long  baseline  navigation,  single-beacon  navigation  reduces  the  system 
complexity  at  the  expense  of  reduced  dimensionality  of  the  constraint  on  vehicle  position 
for  each  measurement.  By  virtue  of  requiring  only  one  beacon,  single -beacon  navigation 
requires  less  equipment  than  traditional  long-baseline.  Additionally,  the  implementation 
of  single -beacon  navigation  used  in  this  thesis  relies  only  on  a  ship-based  beacon,  elimi¬ 
nating  the  costly  and  time-consuming  deployment  and  beacon  survey.  However,  a  single 
range  measurement  provides  no  position  information  in  the  direction  orthogonal  to  the  di¬ 
rection  of  the  range  measurement,  relying  on  multiple  measurements  at  different  times  and 
at  different  relative  bearings  to  provide  a  fully  observable  solution. 

Observability  in  single-beacon  navigation  has  been  covered  from  several  perspectives 
as  described  in  Appendix  A. 5.  This  thesis  will  not  discuss  the  details  of  observability,  but 
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the  consensus  is  that  vehicle  position  is  globally  observable  provided  vehicle  heading  is 
known,  the  vehicle  is  not  travelling  directly  towards  or  away  from  the  beacon,  and  at  least 
three  range  measurements  have  been  made  that  are  not  along  a  linear  vehicle  trajectory  (i.e. 
range  measurements  have  been  made  at  three  non-collinear  points). 

The  remainder  of  this  section  describes  single -beacon  one-way  travel-time  navigation, 
the  implementation  of  single -beacon  navigation  advanced  in  this  thesis,  followed  by  a  re¬ 
view  of  prior  research  on  single-beacon  navigation. 

2.2.1  Single-Beacon  One- Way-Travel-Time  Navigation 

Single-beacon  one-way  travel-time  (OWTT)  navigation  relies  on  range  estimation  from 
the  time-of-flight  (TOF)  of  acoustic  data  packets  propagating  between  a  reference  beacon 
with  a  known,  though  not  necessarily  stationary,  location  and  the  underwater  vehicle  to 
provide  a  reference  to  the  world  frame  [27,28].  The  implementation  of  OWTT  naviga¬ 
tion,  discussed  in  detail  in  Chapter  3,  requires  underwater  acoustic  modems  on  both  the 
reference  beacon  and  the  underwater  vehicle,  as  well  as  precision  clocks  to  synchronize  the 
modems.  Figure  2.1  depicts  a  ship-based  acoustic  modem  broadcasting  acoustic  data  pack¬ 
ets  to  multiple  underwater  vehicles.  The  acoustic  data  packets  broadcast  by  the  reference 
beacon  (in  this  case  a  ship)  encode  both  the  time-of-launch  (TOL)  and  information  about 
the  geodetic  location  of  the  sender’s  transducer  at  the  TOL.  The  time-of-arrival  (TOA)  of 
this  acoustic  data  packet  at  the  receiver,  combined  with  the  decoded  TOL  and  the  position 
information  in  the  acoustic  data  packet,  are  used  to  estimate  range.  A  range  measurement, 
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Figure  2.1:  Acoustic  data  packet  broadcast  from  the  ship  to  multiple  vehicles  (RIP  ABE). 
Photo  credit:  Paul  Oberlander,  WHOI. 


in  conjunction  with  the  depth  of  the  vehicle  measured  from  a  pressure  sensor,  constrains 


the  vehicle  position  to  a  circle  of  solutions.  Between  range  measurements,  relative  vehicle 


motion  is  estimated  using  velocity  and  attitude  measurements. 
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The  one-way  time-of-flight  is  calculated  from  the  difference  between  the  TOL  encoded 
in  the  acoustic  packet,  measured  by  a  precision  clock  and  modem  system  aboard  the  ship, 
and  the  TOA,  measured  by  a  precision  clock  and  modem  system  aboard  the  underwater 
vehicle.  The  accuracy  of  this  TOF  measurements  is  limited  by  the  accuracy  of  the  precision 
clocks  residing  on  the  ship  and  the  underwater  vehicle.  To  ensure  valid  TOF  measurements, 
it  is  crucial  that  the  clocks  on  the  sender  and  receiver  remain  synchronized  throughout  the 
dive  to  within  an  acceptable  tolerance.  The  time-keeping  problem  and  our  solution  are 
discussed  in  more  detail  in  Section  3.2.2. 

OWTT  navigation  provides  bounded-error  position  estimates.  Moreover,  when  the  ship 
and  vehicle  navigate  in  concert,  navigation  can  be  conducted  on  an  unbounded  area.  This 
is  in  contrast  to  a  conventional  12  kHz  LBL  system  using  fixed  beacons  that  provides 
navigation  only  within  a  range  of  a  5-10  km  radius  from  the  beacons.  OWTT  navigation 
provides  scalability  as  well,  allowing  any  vehicles  within  acoustic  range  to  simultaneously 
use  the  same  acoustic  data  packet  broadcast  independent  of  the  number  of  vehicles. 

2.2.2  Prior  Single-Beacon  Navigation  Research 

An  in-depth  review  of  selected  single-beacon  navigation  papers — including  a  summary 
of  the  observation  and  process  models  used  and  a  summary  of  the  authors’  conclusions — 
is  provided  in  Appendix  A.  This  section  provides  a  more  inclusive,  though  less  detailed, 
overview  of  prior  research  in  single-beacon  navigation. 

The  majority  of  prior  literature  in  single -beacon  navigation  reports  estimation  algo- 
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rithms  and  the  results  of  numerical  simulations  of  these  algorithms.  Only  a  few  report 
experimental  evaluations  of  the  proposed  algorithms,  and  even  fewer  employ  independent 
navigation  methods  to  evaluate  quantitatively  the  accuracy  of  the  proposed  methods. 

The  earliest  formulation  of  vehicle  navigation  using  ranges  from  a  single  beacon  that  is 
known  to  the  authors  is  reported  in  [80].  This  approach  employs  least-squares  to  estimate 
the  unknown  initial  vehicle  position  and  a  constant-velocity  unknown  current;  additionally, 
a  linear  algebra-based  observability  analysis  is  reported. 

Range-only  localization  methods  used  for  estimating  the  position  of  a  target  are  ad¬ 
dressed  by  [76]  and  [85].  In  [76]  the  authors  assume  a  constant-velocity,  constant-bearing 
target  trajectory  and  compute  the  theoretical  Cramer-Rao  bound  and  compare  it  to  the  per¬ 
formance  of  a  maximum-likelihood  estimator  (MLE),  an  extended  Kalman  filter  (EKF), 
and  a  regularized  particle  filter  during  field  tests.  In  [85]  the  author  assumes  a  target  with 
constant  acceleration  and  addresses  the  observability  of  the  target-tracker  problem  using  the 
Fisher  information  matrix  and  reports  simulation  results  using  an  EKF.  In  related  work,  [1] 
implements  the  EKF  from  [85]  and  reports  simulation  results. 

Several  different  methods  for  addressing  the  observability  of  single-beacon  navigation 
are  reported  in  the  literature.  The  papers  [32-35]  report  an  observability  analysis  employ¬ 
ing  limiting  systems  to  assess  uniform  observability,  and  derive  sufficient  conditions  for 
the  existence  of  an  observer  with  exponentially  decaying  estimation  error  for  the  cases  of 
both  known  and  unknown  ambient  currents.  The  authors  report  field  results  from  their 
implementation  of  an  EKF.  In  related  work  [62]  extends  the  EKF  reported  in  [32-35]  to 
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three-dimensional  coordinates  with  simulation  results. 

A  concise  observability  analysis  in  continuous  time  is  reported  in  [77]  using  Lie  deriva¬ 
tives  to  compute  conditions  for  which  the  system  has  local  weak  observability.  In  [48]  the 
authors  report  an  algebraic  analysis  showing  local  uniform  observability  based  on  signal 
estimation  techniques,  though  the  lack  of  an  estimation  model  disallows  the  computation 
of  an  updated  position  in  the  absence  of  a  new  measurement. 

The  use  of  EKFs  for  homing  and  single-beacon  navigation,  initialized  by  least-squares, 
is  reported  in  [3,4,89]  with  both  simulation  and  field  trials.  Similarly,  in  [18]  the  authors  re¬ 
port  an  EKE  initialized  by  a  minimum  mean  squared  error  solution  with  simulation  results. 
In  [4]  the  authors  also  report  a  simulated  two-vehicle  system  using  a  cascaded  approach 
in  which  the  second  vehicle  navigates  relative  to  the  first  vehicle  using  inter-vehicle  range 
measurements. 

The  papers  [58-60]  report  an  error  state  EKF  for  single -beacon  navigation  based  on 
error  models  of  the  vehicle’s  inertial  navigation  system.  The  authors  report  results  using  a 
combination  of  field  and  simulation  data. 

More  recent  least-squares  solutions  are  reported  in  [65],  [42],  and  [57].  In  [65]  the 
authors  report  a  nonlinear  least  mean  square  method  for  estimating  a  vehicle’s  initial  posi¬ 
tion  after  which  it  relies  on  dead  reckoning.  In  [42]  the  author  reports  an  ad  hoc  iterative 
technique  to  estimate  course.  In  [57]  the  author  reports  a  method  for  advancing  multiple 
single-beacon  fixes  along  the  vehicle’s  estimated  trackline  to  simulate  a  multi-beacon  fix. 

An  extended  set-valued  observer  is  reported  in  [63].  The  authors  show  this  observer 


16 


CHAPTER  2.  UNDERWATER  VEHICLE  NAVIGATION 


provides  bounds  on  the  estimation  error  in  the  presence  of  non-linearities  in  the  model 
and  non-Gaussian  noise,  guaranteeing  that  the  true  vehicle  position  is  contained  within 
the  estimator’s  predicted  error  covariance  ellipsoid  when  linearization  error  and  noise  are 
correctly  characterized. 

Single-beacon  navigation  using  differential  Doppler  measurements  is  reported  in  [17] 
with  simulation  results.  Navigation  using  range-rate  single-beacon  measurements  is  dis¬ 
cussed  in  [81].  In  [83]  the  authors  describe  and  provide  experimental  results  for  several 
methods  of  single-  and  multi-beacon  navigation,  including  ranges  based  on  one-way  travel¬ 
time  measurements  from  one  or  two  beacons  and  the  time-difference  of  arrival  from  multi¬ 
ple  synchronized  beacons. 

This  thesis  is  concerned  with  advancing  the  single -beacon  navigation  method  first  de¬ 
scribed  in  [27]  and  [28],  in  which  the  authors  report  the  theory  and  first  experimental  results 
in  single-beacon  one-way-travel-time  (OWTT)  acoustic  navigation  with  an  acoustic  mo¬ 
dem  and  precision  timing  board.  The  authors  employ  a  maximum-likelihood  estimator  and 
report  field  results  from  shallow-water  sea  trials.  This  thesis  extends  OWTT  navigation  to 
a  centralized  EKF  and  includes  results  from  deep-water  field  trials  as  described  in  Chapter 
5  and  reported  in  part  in  [93].  This  thesis  also  extends  OWTT  navigation  to  a  decentralized 
extended  information  filter  (EIF)  and  includes  simulation  results  as  described  in  Chapter  6 
and  reported  in  part  in  [94] . 

The  single-beacon  navigation  method  reported  that  most  closely  resembles  the  work 
presented  in  this  thesis  is  [29].  In  [29]  the  authors  report  a  system  employing  a  single  mov- 
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ing  georeferenced  beacon  to  support  the  localization  of  multiple  vehicles  through  asyn¬ 
chronous  acoustic  broadcasts.  This  work  is  a  single -beacon  implementation  of  the  decen¬ 
tralized  multi-vehicle  navigation  method  reported  in  [5,  6],  which  is  discussed  briefly  in 
Section  2.3  and  in  more  detail  in  Section  6.4.3.  The  principal  difference  between  [29]  and 
the  decentralized  OWTT  navigation  algorithm  reported  herein  is  that  [29]  uses  an  ad  hoc 
vehicle-based  EKF  to  perform  range  measurement  updates  that  rely  on  the  absolute  position 
and  covariance  broadcast  from  the  reference  beacon.  In  contrast,  the  decentralized  method 
reported  herein  relies  on  incremental  data  broadcast  from  the  reference  beacon,  allowing  us 
to  exactly  recreate  the  results  of  a  centralized  EKE  that  has  access  to  measurements  from 
both  the  vehicle  and  the  beacon’s  navigation  sensors. 

2.3  Decentralized  Estimation  in  Navigation 

Decentralized  estimation  in  the  context  of  underwater  communication  and  navigation 
faces  unique  constraints  in  terms  of  low  bandwidth  and  high  latency,  which  renders  many 
of  the  decentralized  estimation  solutions  from  land-based  applications  unsuitable.  Until 
recently  little  research  has  been  done  on  the  topic  of  decentralized  estimators  and  multi¬ 
vehicle  navigation  in  the  field  of  underwater  robotics.  However,  as  the  cost  of  vehicles 
has  decreased  and  their  reliability  improved,  increased  interest  in  multi-vehicle  operations 
within  the  ocean  science  community  has  precipitated  new  research  in  this  area. 

The  authors  of  [5, 6]  address  cooperative  localization  of  multiple  underwater  and  sur- 
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face  vehicles  in  a  SLAM  framework  using  ranges  between  multiple  vehicles  and  reference 
beacons.  The  method  presented  allows  a  vehicle  running  a  Bayes  estimator  to  use  range 
and  position  information  broadcast  from  one  or  more  moving  beacons;  the  mechanics  of 
the  algorithm  are  discussed  in  more  detail  in  Section  6.4.  This  work  expands  on  the  moving 
long  baseline  concept  in  [90]  to  encompass  multiple  range  sources  and  real-time  operation. 
The  authors  of  [69]  address  a  similar  concept  to  moving  long  baseline  and  compare  the 
effect  of  the  use  of  the  Kalman  filter  versus  the  particle  filter  on  the  vehicle’s  localization 
performance. 

2.4  Navigation  using  Information  Filters 

To  date,  the  information  filter  has  not  been  widely  used  for  navigation  in  the  field  of 
robotics.  Derived  in  detail  in  [70],  the  extended  information  filter  (EIF)  has  been  employed 
for  vehicle  navigation  [15]  and  SLAM  algorithms  [74,  87]  in  the  context  of  land-based 
robotics.  In  the  context  of  underwater  vehicles,  the  EIF  is  most  widely  used  for  coordinated 
control,  but  there  are  a  few  examples  of  the  EIF  being  employed  in  SLAM  algorithms 
[22,26]. 
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2.5  Range-Only  Simultaneous  Localization  and 
Mapping 

Though  not  directly  related  to  single-beacon  navigation,  range-only  simultaneous  local¬ 
ization  and  mapping  (RO-SLAM)  is  briefly  reviewed  here.  RO-SLAM  is  concerned  with 
estimating  vehicle  position  relying  solely  on  range  information  from  multiple  beacons.  The 
extension  of  RO-SLAM  to  multiple  vehicles  is  similar  to  the  future  work  proposed  in  this 
thesis — incorporating  inter-vehicle  ranges  from  multiple  vehicles  into  a  decentralized  nav¬ 
igation  solution. 

The  authors  of  [71,72]  address  RO-SLAM  for  underwater  vehicles  and  report  exper¬ 
imental  results.  In  this  formulation  multiple  beacons  are  used  but  a  priori  beacon  loca¬ 
tion  is  not  known.  Multi-beacon,  range-only  navigation  for  terrestrial  vehicles  in  a  SLAM 
framework  is  addressed  in  [23, 24, 51, 52, 55, 56]  using  radio-frequency  beacons  for  range 
measurement,  in  [64]  using  audible  sound,  in  [67, 86]  using  wireless  sensor  networks,  and 
in  [9, 10]  with  an  unspecified  range  sensor.  Specifically  in  the  context  of  wireless  networks, 
the  author  of  [61]  computes  the  Cramer-Rao  bound  on  positioning  accuracy  using  range 
measurements  between  multiple  agents  and  illustrates  with  simulation. 
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Chapter  3 

A  Platform-Independent  Acoustic 
Communication  and  Navigation  System 

3.1  Introduction 

This  chapter  describes  a  platform-independent  acoustic  communication  system,  re¬ 
ferred  to  as  the  Acomms  system,  designed  by  the  author  and  collaborators  to  enable  mul¬ 
tiple  nodes  (any  combination  of  underwater  vehicles,  surface  ships,  and  fixed  beacons) 
to  simultaneously  exchange  data  and  calculate  inter-node  ranges  with  one  meter  accuracy 
with  up  to  10  km  range.  The  Acomms  system  and  field  results  herein  are  reported  in  part 
in  [92], 

Multi-vehicle  operations  are  motivated  by  the  desire  to  collect  richer  data  sets,  i.e. 
increased  spatial  extent,  spatial  resolution,  and/or  the  variety  of  data  types.  The  com- 
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bined  communication  and  navigation  system  described  herein  supports  one-way-travel¬ 
time  (OWTT)  navigation,  which  enables  one  or  more  vehicles  to  use  a  single,  georefer- 
enced,  moving  beacon,  e.g.  the  ship,  to  perform  bounded-error  navigation.  As  discussed  in 
detail  in  Chapter  2,  bounded-error  navigation  is  achieved  currently  with  the  aid  of  systems 
such  as  long  baseline  (LBL)  or  ultra-short  baseline  (USBL)  navigation.  LBL  navigation 
requires  external,  fixed  reference  beacons  that  have  a  range  of  only  5-10  km  and  require 
additional  survey  and  recovery  time,  while  the  accuracy  of  USBL  navigation  is  approxi¬ 
mately  1%  of  range,  which  limits  its  usefulness  over  long  ranges.  Using  the  ship  as  a  single 
reference  beacon,  OWTT  navigation  enables  the  vehicles  to  travel  over  tens  of  kilometers 
limited  only  by  speed  and  endurance  and  removing  the  need  for  external  beacons.  In  addi¬ 
tion,  the  accuracy  of  the  range  measurements  used  in  OWTT  navigation  is  independent  of 
range. 

Advances  in  underwater  communication  systems  promise  improved  communication 
and  connectivity  for  underwater  vehicles.  Acoustic  communication  systems  are  increas¬ 
ingly  employed  on  untethered  underwater  vehicles,  which  have  historically  had  limited 
telemetry  when  submerged.  The  Acomms  system  supports  two  types  of  acoustic  com¬ 
munication:  asynchronous  communication,  which  is  the  most  commonly  used  for  sending 
data,  and  synchronous  communication,  which  in  addition  to  communication  enables  navi¬ 
gation  using  inter-node  ranges  derived  from  the  one-way  travel-times  of  acoustic  messages 
between  nodes. 

The  Acomms  system  hardware  is  implemented  with  a  dedicated  software  program, 
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Linux  host  computers,  acoustic  underwater  modems,  and  precision  reference  clocks.  The 
acoustic  communications  software  configures  the  modem,  manages  all  acoustic  communi¬ 
cation  traffic,  and  acts  as  an  interface  between  the  vehicle-specific  software  and  the  modems 
and  clocks.  While  the  communications  and  one-way  travel  time  features  are  provided  using 
the  Woods  Hole  Oceanographic  Institution  (WHOI)  Micro-Modems  [30,31]  and  the  PPS- 
Board  [27,28],  the  concepts  have  been  developed  in  a  hardware  independent  framework  and 
can  be  used  with  any  acoustic  system  or  combination  of  systems  that  includes  bidirectional 
communications  with  synchronous  transmission  and  precision  time-tagged  reception. 

The  Acomms  software  and  related  hardware  have  been  installed  on  the  Woods  Hole 
Oceanographic  Institution  vehicles  Puma ,  Jaguar ,  and  Nereus ,  and  have  been  deployed 
successfully  in  sea  trials  at  the  southern  Mid-Atlantic  Ridge  [93],  the  Mariana  Trench  [12, 
13,96],  and  the  Cayman  Trough  [37].  This  chapter  covers  the  system  architecture  in  detail 
and  results  from  each  of  the  field  trials. 

3.2  Acoustic  Communication 

The  Acomms  software,  designed  to  operate  symmetrically  on  all  nodes,  initializes  the 
modem  and  issues  a  sequence  of  modem  commands,  defined  by  the  user,  to  initiate  data 
transmissions  between  nodes,  transmit  ranging  pings,  and  interrogate  acoustic  navigation 
beacons.  In  addition,  the  Acomms  software  enables  the  user  to  specify  modem  configu¬ 
rations  and  ensures  that  the  modem  is  properly  configured  after  a  vehicle  or  modem  re- 
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boot.  The  Acomms  software  time-stamps  and  logs  all  modem  communication  traffic  and 
Acomms-related  data,  provides  basic  format  checking  for  messages  sent  to  the  modem, 
monitors  the  state  of  the  modem,  tracks  message  traffic  and  modem  status,  and  reports  mo¬ 
dem  statistics  to  the  vehicle  controller.  Details  of  the  Acomms  software  are  discussed  in 
Section  3.3.4. 


3.2.1  Asynchronous  Communication 

The  majority  of  vehicles  that  use  underwater  modems  operate  their  modems  asyn¬ 
chronously:  the  modems  are  used  to  transmit  data  and  send  commands  between  nodes — 
where  a  node  can  be  an  underwater  vehicle,  a  ship,  or  a  fixed  entity  such  as  a  mooring — 
without  the  need  for  precision  or  synchronized  time-keeping  among  the  nodes.  During 
asynchronous  operation,  a  modem  on  one  node  is  typically  designated  as  the  master  mo¬ 
dem.  The  master  modem  initiates  acoustic  communications  for  all  nodes  in  a  deployment, 
eliminating  the  potential  for  collisions  between  acoustic  data  transmissions. 

3.2.2  Synchronous  Communication  and  Navigation 

In  addition  to  asynchronous  communication  capabilities,  the  Acomms  system  enables 
synchronous  communication  and  navigation  when  equipped  with  a  precision  clock.  For 
synchronous  communication,  topside  nodes,  i.e.  nodes  that  are  not  submerged,  rely  on  a 
global  positioning  system  (GPS)  timeserver  for  their  timing  reference.  Subsea  each  node  is 
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equipped  with  a  free-running  precision  clock  as  a  timing  reference  that  is  synchronized  to 
a  GPS  timeserver  prior  to  the  start  of  the  dive.  Details  of  the  implementation  are  covered 
in  Section  3.3. 

OWTT  navigation,  first  proposed  in  [27],  uses  one-way  travel  times  of  acoustic  mes¬ 
sages  to  estimate  range  between  subsea  nodes  and  a  surface  node,  such  as  a  vehicle  or 
ship  equipped  with  a  GPS  receiver,  that  has  knowledge  of  its  position  in  the  world  frame 
[27,28,93].  OWTT  navigation  employs  acoustic  broadcasts  of  data  packets  that  contain 
information  about  the  transmitter’s  position  and  the  time  at  which  the  message  was  trans¬ 
mitted.  Because  the  transmitter  and  receiver  clocks  are  synchronized,  the  receiver  can 
calculate  the  time-of-flight  of  the  acoustic  broadcast  using  the  time-of-arrival  of  the  mes¬ 
sage  and  the  time-of-launch  that  is  encoded  in  the  data  packet.  Time-of-flight  information 
combined  with  the  acoustically  encoded  position  information  from  the  transmitter  provides 
a  range  measurement  from  a  known  position  in  the  world  frame.  Between  range  mea¬ 
surements  the  vehicle  performs  dead  reckoning.  OWTT  navigation  is  covered  in  detail  in 
Chapter  2  and  [27],  [28],  and  [93]. 

The  Acomms  software  supports  OWTT  navigation  through  a  message  packing  function 
that  precisely  controls  the  timing  of  messages  provided  to  the  modem  as  specified  in  [39] 
and  can  thus  properly  anticipate  and  encode  the  time-of-launch  of  the  data  packet.  The 
Acomms  software  and  hardware  also  ensures  that  the  modem’s  internal  clock,  which  is 
used  to  measure  the  time-of-arrival  of  messages,  is  properly  disciplined. 

In  addition  to  enabling  OWTT  navigation,  synchronous  communication  provides  fur- 
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ther  advantages  over  asynchronous  communication  by  making  it  possible  to  accurately 
predict  the  timing  of  acoustic  transmissions  of  other  nodes,  thereby  enabling  synchronized 
time-division  multiple  access  (TDMA)  cycles  among  nodes.  Typically,  to  avoid  collision 
of  acoustic  messages,  a  single  node  is  designated  as  the  master  that  commands  data  trans¬ 
missions  for  all  nodes  in  a  deployment.  Synchronous  communication  eliminates  the  need 
for  a  single  master  node  because  messages  originating  at  different  nodes  can  be  scheduled 
a  priori  to  not  overlap.  This  feature,  which  was  used  extensively  during  field  trials  as  de¬ 
scribed  in  Section  3.4,  enables  a  higher  throughput  of  data  while  also  retaining  the  ability 
for  an  operator  to  transmit  additional  acoustic  messages  from  the  ship.  Using  synchronous 
communication,  an  operator  knows  the  exact  timing  of  the  vehicle’s  sequence  of  acoustic 
transmissions  and  can  reliably  predict  when  the  acoustic  channel  is  clear  for  transmission 
from  the  ship,  such  as  an  abort  message  that  commands  the  vehicle  to  return  to  the  sur¬ 
face.  For  example,  a  vehicle’s  modem  could  be  programmed  to  run  a  two-minute-long 
cycle  of  data  transmissions  that  starts  on  the  even  minutes  but  begins  with  a  thirty-second 
period  during  which  no  acoustic  transmissions  are  scheduled.  The  shipboard  operator  then 
knows  that  the  thirty-second  period  after  every  even  minute  is  clear  to  transmit  acoustic 
transmission  from  the  ship  to  the  vehicle  without  risking  message  collision.  We  have  fur¬ 
ther  exploited  this  functionality  to  enable  on-the-fly  switching  between  different  modem 
configurations  (e.g.  frequency  band,  modulation  method,  bandwidth,  and  bitrate)  as  de¬ 
scribed  in  more  detail  in  Section  3.3.1,  which  requires  simultaneously  reconfiguring  both 
the  transmitting  and  receiving  modems. 
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Previously  reported  acoustic  modem  drivers  for  synchronous  communication,  such  as 
the  modem  drivers  employed  on  the  Massachusetts  Institute  of  Technology  (MIT)  au¬ 
tonomous  surface  vehicles  [20],  and  the  WHOI  Seabed  vehicles  [27,28],  are  not  portable 
due  to  the  tight  integration  of  these  modem  drivers  into  the  vehicle-specific  application 
code  of  their  respective  vehicle  control  and  navigation  systems.  In  contrast,  the  Acomms 
software  reported  herein  is  portable,  employs  a  vehicle-independent  interface  based  on  user 
datagram  protocol  (UDP)  messages,  runs  as  a  stand-alone  daemon  on  a  host  Linux  CPU, 
and  operates  symmetrically  on  all  node  types — e.g.  underwater  vehicles,  fixed  beacons, 
and  surface  ships. 

3.3  System  Architecture 

For  asynchronous  communication  the  Acomms  system  requires  an  acoustic  underwater 
modem,  a  transducer,  and  a  host  computer  at  each  node.  Synchronous  communication  addi¬ 
tionally  requires  that  each  node’s  computer  be  synchronized  within  an  acceptable  tolerance 
for  the  duration  of  the  mission.  For  example,  assuming  a  sound  speed  of  1500  m/s,  a  10  ms 
offset  between  the  sender  and  receiver  results  in  a  15  m  error  in  the  range  measurement. 
The  architecture  of  the  Acomms  system  in  a  typical  two-node  setup  is  depicted  in  Figure 
3.1,  where  the  vehicle  is  referred  to  as  the  subsea  node  and  the  ship  is  referred  to  as  the 
topside  node.  Topside  the  Acomms  software  runs  on  a  laptop  running  Ubuntu  Linux  and 
communicates  with  the  shipboard  modem  via  the  network  using  UDP  messages  through 


27 


CHAPTER  3.  ACOUSTIC  COMMUNICATION  AND  NAVIGATION  SYSTEM 


Figure  3.1:  Typical  sea-going  architecture  for  a  two-node  deployment  of  the  Acomms  sys¬ 
tem. 

a  MOXA™serial  device  server.  Computers  on  the  topside  network  are  synchronized  to 
a  GPS  timeserver  through  the  Network  Time  Protocol  (NTP).  In  addition,  the  timeserver 
supplies  a  pulse-per-second  (PPS)  signal  to  the  topside  modem,  which  consists  of  a  1  Hz 
square  wave  that  has  its  rising  edge  synchronized  with  the  start  of  the  second.  During  the 
field  trials  described  here  we  used  a  Meinberg  GPS/NTP  shipboard  timeserver  [66] . 

Subsea,  the  Acomms  software  runs  on  the  main  vehicle  computer,  also  running  Ubuntu 
Linux,  and  communicates  with  the  modem  over  a  serial  connection.  Acomms  communi¬ 
cates  with  the  vehicle’s  controller  and  navigation  processes  over  the  vehicle’s  local  network 
using  UDP  messages.  At  present,  no  commercially  available  precision  clocks  (such  as 
the  Meinberg  noted  above)  are  suitable  for  use  on  small  autonomous  underwater  vehicles 
(AUVs)  due  to  power  and  size  constraints.  To  address  this,  we  use  the  PPSBoard — a  small, 
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comparatively  low-power,  precision  clock  board  suitable  for  AUV  applications  [27,28]. 
The  PPSBoard  on  the  vehicle  serves  as  the  vehicle’s  on-board  NTP  timeserver  and  pro¬ 
vides  a  PPS  signal  to  the  vehicle’s  modem.  The  acoustic  modem  used  in  the  field  trials 
described  here  is  the  WHOI  Acoustic  Micro-Modem.  The  individual  components  of  the 
Acomms  system  are  described  in  detail  below. 

3.3.1  WHOI  Micro-Modem 

The  WHOI  Micro-Modem  is  an  acoustic  modem  capable  of  encoding  and  decoding 
acoustic  data  packets  that  it  transmits  through  the  water  column  [30,31].  All  Micro- 
Modems  are  able  to  transmit  both  frequency-shift  keyed  (FSK)  and  phase-shift  keyed 
(PSK)  encoded  acoustic  messages.  All  Micro-Modems  are  able  to  receive  FSK  encoded 
acoustic  messages  and,  with  the  addition  of  a  coprocessor  board,  are  also  able  to  receive 
PSK  encoded  messages.  In  Band  A  (8-12  kHz  carrier  frequency)  32-byte-long  FSK- 
encoded  data  packets  take  3-4  seconds  to  transmit.  Mini-packets  (32  bits  long)  take  840 
ms  to  transmit.  The  range  of  the  Micro-Modem  varies  with  encoding,  bandwidth,  data-rate 
and  the  acoustic  channel  characteristics  (horizontal/shallow  channel  versus  vertical/deep 
channel).  During  recent  trials  in  the  Mariana  Trench,  the  author  and  collaborators  tested 
the  modem’s  capabilities  to  the  extremes  of  the  vertical  channel  for  a  variety  of  combina¬ 
tions  of  encoding,  bandwidth,  and  data-rates.  PSK  encoded  data  packets  broadcast  by  the 
vehicle’s  modem  at  the  lowest  data-rate  were  reliably  received  by  the  ship’s  modem  at  up 
to  11  km  [84], 
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The  Micro-Modem  employs  its  own  internal  clock  to  calculate  the  time-of-arrival  of 
acoustic  messages  and  the  travel-time  of  ranging  pings  and  replies  from  acoustic  navigation 
beacons.  When  the  modem  is  in  synchronous  navigation  (SNV)  mode,  as  described  in  [39], 
the  modem’s  clock  can  be  synchronized  to  a  PPS  signal  using  a  NMEA  clock  message 
from  the  host.  Once  synchronized,  the  time-of-arrival  (TOA)  of  each  arriving  message 
is  reported  to  have  an  accuracy  of  ±  125  /is  with  respect  to  the  PPS  signal  [31].  The 
accuracy  of  the  PPS  signals  used  topside  and  subsea  are  discussed  in  Sections  3.3.2  and 
3.3.3  respectively.  In  SNV  mode,  all  transmitted  messages  are  initiated  by  the  modem 
within  ±  10  /is  of  the  rising  edge  of  the  PPS  signal  [39]. 

3.3.2  PPSBoard 

The  PPSBoard  provides  a  stable  time  reference  that  keeps  the  undersea  vehicle’s  CPU 
clock  and  the  vehicle’s  modem  synchronized  with  the  topside  clock  throughout  the  mis¬ 
sion.  The  PPSBoard,  described  in  detail  in  [27]  and  [28],  was  developed  by  Eustice  and 
Whitcomb  to  provide  a  free-running,  precision  timing  reference  for  use  subsea  that  can  be 
synchronized  to  a  GPS  timing  signal.  In  addition  to  supplying  a  PPS  signal  to  the  Micro- 
Modem  to  enable  its  SNV  mode  described  above,  the  PPSBoard  is  used  to  discipline  the 
vehicle  CPU’s  NTP  server  by  providing  a  PPS  signal  and  a  NMEA-formatted  clock  mes¬ 
sage  naming  the  upcoming  second.  The  PPSBoard  is  synchronized  to  a  GPS  signal  while 
the  vehicle  is  on  deck,  and  the  drift  characteristics  of  the  board  (~1  ms  drift  over  14  hours) 
ensure  that  the  error  introduced  in  the  estimated  range  between  the  ship  and  the  vehicle  due 
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to  the  relative  drift  between  the  two  clocks  is  small — 1  ms  clock  drift  corresponds  to  1.5  m 
error  in  the  range  between  the  ship  and  the  vehicle  assuming  1500  m/s  sound  velocity. 

3.3.3  Topside  NTP  Timeserver 

The  Meinberg  GPS/NTP  timeserver,  used  during  the  field  trials  to  provide  a  stable, 
shipboard  timing  reference,  is  a  Stratum- 1  NTP  timeserver  [66].  The  topside  computer 
stays  synchronized  with  the  timeserver  over  the  network  via  NTP.  The  Meinberg  also 
supplies  a  PPS  signal,  accurate  to  within  10  /is,  to  the  topside  modem. 

3.3.4  Acomms  Software 

The  Acomms  software,  written  by  the  author  with  assistance  from  Louis  L.  Whitcomb, 
is  a  multi-threaded  program  written  in  C/C++  that  uses  modem  driver  code  written  by 
Matthew  Grund.  The  Acomms  software  executes  a  state  machine  consisting  of  two  sec¬ 
tions:  a  modem  initialization  section  and  a  TDMA  sequence  of  commands.  It  is  designed 
to  act  as  a  transport  layer  between  the  host  computer  and  the  modem,  passing  through  all 
message  traffic  in  both  directions.  All  communications  with  the  modem  as  well  as  various 
statistics  on  messages  transmitted  and  received  are  time-stamped  and  logged.  In  addition, 
the  Acomms  software  enables  synchronous  communication  and  navigation  as  described 
below. 

The  Acomms  software  is  designed  to  run  as  a  stand-alone  process  in  either  the  fore- 
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ground  or  the  background,  communicating  with  the  modem  and  other  processes  via  serial 
or  network  connections.  The  software  is  also  able  to  run  as  a  daemon  thanks  to  Giancarlo 
Troni  and  Clayton  Kunz.  On  Nereus ,  the  Acomms  daemon  is  started  during  the  host  com¬ 
puter  boot-up  process  to  ensure  that  the  daemon  is  always  running  even  in  the  event  that 
the  vehicle  computer  reboots. 

Serial  and  Network  Connections 

The  Acomms  software  allows  the  user  to  specify  the  connections  to  the  modem  and 
the  host  computer  as  shown  in  Figure  3.1.  The  Acomms  software  supports  both  serial  I/O 
connection  and  network  I/O  connections.  Network  I/O  connections  use  UDP  and  can  either 
broadcast  or  unicast  to  a  specified  UDP  port.  A  typical  setup  uses  a  serial  connection  to  the 
modem  on  the  vehicle  and  a  network  connection  to  the  topside  modem.  On  the  vehicle  the 
software  typically  communicates  with  the  vehicle  control  and  navigation  processes  over  the 
network.  The  software  is  also  able  to  route  specific  types  of  modem  messages  to  different 
processes.  For  example  one  network  I/O  thread  can  communicate  with  the  main  vehicle 
process  while  a  second  network  EO  thread  communicates  with  the  navigation  process. 

Modem  Initialization 

The  Acomms  software  has  a  user-configurable  initialization  file  that  supports  all  of 
the  WHOI  Micro-Modem  configurations.  Configuration  commands  are  sent  to  the  modem 
when  the  Acomms  process  is  started,  and  every  time  the  Acomms  process  receives  a  mes- 
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sage  from  the  modem  indicating  that  the  modem  has  rebooted,  in  order  to  ensure  that  the 
modem  stays  properly  configured. 


TDMA  Cycle 

The  Acomms  software  supports  a  user-configured  TDMA  cycle  of  modem  commands 
that  is  executed  continuously  except  when  interrupted  by  the  modem  initialization  process 
or  the  clock  watchdog  (described  below).  The  TDMA  cycle  is  used  to  command  the  modem 
to  transmit  acoustic  messages,  interrogate  long  baseline  (LBL)  beacons,  or  change  selective 
modem  configurations  on-the-fly  such  as  the  transmission  frequency  and  bandwidth.  Table 
3.1  shows  the  TDMA  commands  currently  available  in  the  Acomms  software. 


Table  3.1:  TDMA  Cycle  Command  Summary 


Command 

Size 

Addt’l  Info 

Configuration 

n/a 

supports  all  available  cfgs 

Ranging  Ping 

32  bits 

returns  OWTT  between  nodes 

Mini-Packet 

32  bits 

user  specified  codes  (e.g.  Abort,  UnAbort,  etc.) 

Cycle-Init 

32  bits 

initiates  data  TX  between  nodes 

Data  Packet 

32  —  2048  bytes 

user  specified  data,  length  varies  by  encoding  type 

LBL 

n/a 

listens  on  up  to  4  frequencies 

PAUSE 

n/a 

see  Section  3.3.4 

VLPAUSE 

n/a 

see  Section  3.3.4 

The  commands  PAUSE  and  VLPAUSE  are  special  commands  that  pause  the  TDMA 
cycle  before  continuing  to  the  next  entry.  PAUSE  stops  the  TDMA  cycle  for  a  specified 
number  of  seconds,  VLPAUSE  pauses  the  TDMA  cycle  for  a  variable  length  of  time  in 
order  to  restart  the  TDMA  cycle  at  an  exactly  specified  interval  (e.g.  at  the  beginning  of 
every  even  minute).  These  commands  are  used  to  insert  time  into  the  TDMA  cycle  so  that 
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the  acoustic  channel  is  clear  for  the  modem  to  receive  messages  initiated  at  another  node 
and,  in  the  case  of  VLPAUSE,  to  synchronize  the  TDMA  cycle  with  the  vehicle  clock. 

Each  entry  in  the  TDMA  cycle  has  an  associated  timeout  period  and  a  retry  flag,  such 
that  if  the  step  is  not  successfully  completed  by  the  end  of  the  timeout  period,  the  TDMA 
cycle  will  either  proceed  with  sending  the  next  message  in  the  cycle  or  resend  the  current 
message  as  dictated  by  the  retry  flag.  The  Acomms  software  TDMA  cycle  is  designed  such 
that  upon  the  successful  completion  of  an  entry  in  the  TDMA  cycle,  the  software  can  either 
proceed  directly  to  the  next  entry,  or  it  can  wait  for  the  full  timeout  specified  for  that  entry 
(fixed  interval  timeout  mode).  For  operations  where  higher  message  transmission  rates  are 
desired,  the  former  is  used.  For  operations  where  it  is  desirable  to  keep  the  TDMA  cycle 
synchronized  to  the  clock,  the  latter,  in  combination  with  a  VLPAUSE,  is  used. 

Synchronous  Navigation  Mode  and  Clock  Watchdog 

In  order  for  the  Acomms  software  to  support  the  Micro-Modem’s  synchronous  naviga¬ 
tion  (SNV)  mode,  the  modem’s  clock  must  be  initialized  and  monitored.  This  is  accom¬ 
plished  by  sending  appropriately  timed  clock  messages  to  the  modem  in  conjunction  with 
the  PPS  signal  as  described  in  [39].  The  TDMA  cycle  sets  the  clock  when  Acomms  is 
started,  after  every  modem  reboot,  and  whenever  the  clock  watchdog  is  triggered  by  one  of 
several  indications  that  the  modem  clock  needs  to  be  set. 

When  the  modem  is  in  SNV  mode,  messages  initiated  by  the  modem  are  transmitted 
at  the  top-of-the-second,  triggered  by  the  rising  edge  of  the  PPS  signal.  The  Acomms 
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software  monitors  the  timing  of  messages  sent  to  the  modem  to  ensure  that  the  messages 
are  provided  adequately  ahead  of  the  top-of-the-second,  as  specified  in  [39],  so  that  they 
can  be  transmitted  at  the  top-of-the-second. 

One  potential  pitfall  of  SNV  mode  is  that  in  the  event  of  the  loss  of  the  PPS  signal, 
no  acoustic  messages  will  be  initiated  by  the  modem.  To  prevent  a  vehicle  from  losing 
all  acoustic  communications  in  this  situation,  a  user-configurable  timeout  is  provided  such 
that  after  a  given  number  of  seconds  without  the  PPS  signal  the  state  machine  will  send  a 
command  to  take  the  modem  out  of  SNV  mode.  Once  the  modem  is  not  in  SNV  mode, 
messages  will  be  sent  regardless  of  the  presence  of  the  PPS  signal.  Message  transmission 
will  not  be  synchronized  with  the  top  of  the  second,  making  range  measurements  from  the 
acoustic  broadcasts  impossible,  but  acoustic  data  transmission  will  resume. 

Support  for  OWTT  Navigation 

To  enable  single-beacon  one-way  travel-time  navigation  as  described  in  Section  3.2.2, 
the  integer  value  of  the  top  of  the  second  at  which  a  data  packet  will  be  transmitted  must 
be  encoded  within  the  data  packet.  Because  the  Acomms  process  controls  the  timing  of 
messages  provided  to  the  modem  as  described  in  Section  3.3.4,  the  Acomms  process  an¬ 
ticipates  the  time-of-launch  of  a  data  packet  and  over-writes  a  designated  byte  in  the  data 
packet’s  payload  with  the  integer  value  of  the  anticipated  time-of-launch.  This  is  the  only 
instance  in  which  the  Acomms  process  will  modify  a  message  sent  from  the  host  process 
before  passing  it  to  the  modem. 
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3.4  Field  Results 

The  Acomms  system  had  been  installed  on  three  of  WHOI’s  underwater  vehicles — the 
hybrid  remotely  operated  vehicle  Nereus  [13]  and  the  Puma  and  Jaguar  AUVs  [82].  To 
date  the  system  has  been  successfully  deployed  on  four  oceanographic  expeditions,  each 
one  allowing  us  to  develop  and  test  new  functionality  for  the  Acomms  system. 

The  Acomms  system  was  first  tested  at  sea  in  October  2007  during  the  deep-water 
field  trials  for  Nereus  near  Oahu,  Hawaii.  During  this  expedition  the  Acomms  system  was 
used  for  asynchronous  communication  between  the  vehicle  and  the  ship,  enabling  ship¬ 
board  AUV  operators  to  monitor  the  progress  of  and  send  commands  to  the  new  vehicle 
while  it  was  submerged  and  operating  autonomously  [14].  In  January  2008  the  Acomms 
system  was  used  for  synchronous  communication  between  the  ship  and  the  AUV  Puma 
or  Jaguar  during  an  expedition  to  locate  and  map  new  hydrothermal  vents  on  the  south¬ 
ern  Mid- Atlantic  Ridge.  The  navigation  results  from  this  expedition  provide  experimental 
validation  in  post-processing  for  OWTT  navigation  [93].  In  May-June  2009  the  Acomms 
system  provided  synchronous  communication  between  three  nodes  ( Nereus ,  the  ship,  and  a 
depressor — described  in  more  detail  below)  during  Nereus ’s  first  dives  to  Challenger  Deep 
in  the  Mariana  Trench  [12].  The  depth  of  the  site  (10,903  m)  and  the  expedition  schedule 
precluded  the  use  of  a  navigation  ground  truth  such  as  LBL,  but  the  multi-node  configu¬ 
ration  highlights  the  benefits  of  synchronous  operation.  In  October  2009,  the  most  recent 
Acomms  deployment  to  date,  the  Acomms  system  was  deployed  with  Nereus  on  an  ex¬ 
pedition  to  locate  and  map  new  hydrothermal  vents  in  the  Cayman  Trough  [37].  During 
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this  expedition,  using  two-node  synchronous  communication,  not  only  were  vehicle  status 
updates  sent  from  the  vehicle  to  this  ship,  but  also  new  mission  plans  and  modifications 
to  existing  mission  plans  were  sent  from  the  ship  to  the  vehicle,  allowing  shipboard  AUV 
operators  to  modify  the  AUVs  survey  path  in  real  time  without  recovering  the  vehicle. 

3.4.1  November  2007,  Nereus ,  Deep-Water  Trials 

Asynchronous  Two-Node  Communication 

The  first  full-scale  at-sea  deployment  of  the  Acomms  system  was  in  November  2007 
during  field  trials  of  the  vehicle  Nereus  near  the  coast  of  Oahu,  Hawaii.  All  Acomms- 
related  hardware  was  installed  on  Nereus  by  Chris  Taylor  and  John  W.  Bailey.  The  author 
installed  the  Acomms  system  software  and  was  responsible  for  Acomms  operations  during 
the  expedition.  Christopher  J.  McFarland  and  Michael  H.  Brown  assisted  with  the  sys¬ 
tem  install  and  testing.  The  Acomms  system  was  used  for  asynchronous  communication 
between  the  vehicle  and  the  ship.  Operations  were  conducted  from  the  R/V  Kilo  Moana, 
a  185’  small  waterplane  area,  twin  hull  (SWATH)  vessel  operated  by  the  University  of 
Hawaii.  Figures  3.2  and  3.3  show  Nereus  being  lowered  into  the  water  from  the  R/V  Kilo 
Moana  configured  as  an  AUV  and  an  ROV  respectively. 

Nereus  is  a  hybrid  remotely  operated  vehicle  (HROV)  that  is  unique  among  full-ocean- 
depth  underwater  vehicles  for  two  reasons:  it  can  be  configured  as  either  a  remotely  oper¬ 
ated  vehicle  or  an  autonomous  underwater  vehicle  and  it  is  designed  to  operate  in  up  to  1 1 
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Figure  3.2:  The  hybrid  remotely  operated  vehicle  Nereus  configured  as  an  AUV.  Photo 
credit:  Louis  Whitcomb,  JHU. 

km  of  water  [13].  Nereus  has  an  EDO/Straza  SP23  transducer  mounted  at  the  forward  end 
of  the  starboard  hull  facing  upwards.  A  second  EDO/Straza  SP23  transducer  was  lowered 
from  the  stem  of  the  ship,  facing  downwards,  in  a  baffled  cage  to  reduce  the  ambient  acous¬ 
tic  noise.  During  initial  tests  the  transducer  was  lowered  from  the  ship  and  held  1-2  meters 
below  the  surface  of  the  water  to  keep  it  clear  of  the  ship’s  propellers  and  other  gear  in  the 
water.  Later  a  bridle  was  used  to  reduce  side-to-side  motion  and  position  the  transducer 
3-4  meters  below  the  surface  of  the  water.  During  two  of  the  dives  Benthos  transponders 
were  also  lowered  from  the  stern  of  the  ship  to  test  the  modem’s  LBL  functionality,  though 
LBL  beacons  were  not  deployed  at  the  site. 

The  Acomms  software  ran  on  the  vehicle  in  both  ROV  and  AUV  mode.  During  this 
expedition,  when  the  vehicle  was  in  ROV  mode,  the  Acomms  software  on  the  ship  was 
typically  designated  as  master  using  a  TDMA  cycle  of  modem  messages  consisting  of  a 
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Figure  3.3:  Lowering  Nereus  off  the  stern  of  the  R/V  Kilo  Moana.  Photo  credit:  Matthew 
Heintz,  WHOI. 

ranging  ping  to  the  vehicle,  a  request  for  a  data  packet  to  be  transmitted  from  the  vehicle 
to  the  ship,  and  an  LBL  ping  if  the  LBL  transducers  were  in  the  water.  When  the  vehicle 
was  in  operation  as  an  AUV,  the  Acomms  software  on  the  vehicle  was  typically  designated 
as  master  using  a  TDMA  cycle  consisting  of  a  data  packet  transmitted  from  the  vehicle  to 
the  ship  and  a  pause  to  allow  for  messages  to  be  transmitted  from  the  ship  to  the  vehicle  if 
necessary.  There  was  no  LBL  ping  because  the  LBL  beacons  were  not  deployed  during  the 
trials.  The  data  packets  transmitted  from  the  vehicle  to  the  ship  contain  vehicle  status  and 
health  updates,  including  estimated  position,  speed,  battery  health,  and  the  current  goal 
or  action.  All  messages  were  FSK-encoded.  The  synchronous  navigation  mode  was  not 
employed  because  the  PPSBoard  was  not  installed  on  the  vehicle  at  that  time. 
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Vehicle  Position  via  Doppler  and  Acoustic  Uplinks 


Figure  3.4:  Real-time  acoustically  reported  vehicle  position  overlaid  on  vehicle  trackline. 

During  the  expedition  three  successful  ROV  dives  were  completed  to  a  maximum  depth 
of  2257  m,  with  a  total  bottom  time  of  7  h  38  min.  As  an  AUV  the  vehicle  completed 
multiple  shallow  dives  over  a  period  of  1 1  h  33  min.  The  vehicle’s  maximum  depth  as  an 
AUV  was  22  m  with  12  min  of  bottom  time  [14,95].  Figure  3.4  shows  the  position  data 
decoded  in  real  time  from  acoustically  transmitted  status  messages  sent  from  the  vehicle 
to  the  ship.  These  position  data  are  overlaid  on  the  full  vehicle  trackline  that  was  retrieved 
after  the  conclusion  of  the  dive.  The  real-time  acoustic  vehicle  status  updates  allowed  the 
AUV  operators  to  monitor  the  vehicle’s  progress  while  it  was  submerged.  In  addition,  we 
successfully  tested  an  acoustic  abort,  where  we  sent  an  abort  code  to  the  vehicle  using  the 
Micro-Modems,  commanding  the  vehicle  to  drop  weights  and  return  to  the  surface. 
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3.4.2  January  2008,  Puma  &  Jaguar ,  Mid- Atlantic  Ridge 
Synchronous  Communication  and  Navigation 

In  January  2008  the  Acomms  system  was  deployed  on  the  Puma  and  Jaguar  AUVs 
during  engineering  trials  for  methods  for  locating  and  mapping  new  hydrothermal  vents 
on  the  southern  Mid-Atlantic  Ridge  in  4000  m  of  water.  Chris  Murphy  assisted  with  the 
integration  of  the  Acomms  software  on  these  AUVs.  The  author  was  responsible  for  the 
Acomms  system  operations  during  the  expedition.  Acoustic  ranges  and  vehicle  navigation 
data  collected  by  the  author  and  collaborators  during  Dive  03  with  Puma  were  used  by  the 
author  and  collaborators  in  post-processing  to  experimentally  validate  the  single -beacon 
one-way  travel-time  navigation  methods  described  in  Section  3.2.2.  Figure  3.5  shows  the 
AUV  Puma',  Figure  3.6  shows  the  trackline  of  the  ship  and  the  estimated  trackline  of  the 
vehicle  during  Dive  03.  This  dive  lasted  21  hours,  of  which  9  hours  (the  portion  shown) 
were  spent  doing  a  gridded  survey  200  m  above  the  seafloor.  Presented  here  is  an  analysis 
of  the  acoustic  throughput  of  the  Acomms  system  during  this  dive.  The  author’s  formula¬ 
tion  of  the  OWTT  navigation  algorithm  and  the  experimental  navigation  results  from  this 
expedition  are  covered  in  Chapter  5. 

Both  Puma  and  Jaguar  have  an  upward-facing  ITC-3013  [45]  transducer  mounted  on 
the  lower  hull  forward  of  the  forward  vertical  yellow  stanchion.  An  additional  ITC-3013 
was  lowered  over  the  side  of  the  ship  3-6  m  below  the  surface  of  the  water  facing  down¬ 
ward.  Both  vehicles  also  have  PPSBoards  installed,  which  enable  the  vehicle  clocks  to  be 
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Figure  3.5:  AUV  Puma.  Photo  credit:  Louis  Whitcomb,  JHU. 


Table  3.2:  Two-minute  TDMA  cycle  of  modem  messages  during  Dive  03. 


Command 

Time  [s] 

Data  TX  to  ship 

20 

PING  ship 

10 

LBL  ping 

20 

Data  RX  from  ship 

20 

PING  ship 

10 

LBL  ping 

20 

no  acoustic  TX 

20 

Total  TDMA  cycle  time  [s] 

120 

synchronized  with  the  topside  clock.  Three  LBL  transducers  were  deployed  at  the  site. 
The  Acomms  software  running  on  the  vehicle  was  designated  as  the  master  and  ran  a  two- 
minute  TDMA  cycle  shown  in  Table  3.2.  All  modem  communications  were  FSK-encoded. 
Travel  times  from  the  LBL  interrogation  were  passed  to  the  navigation  process  on  the  ve¬ 
hicle  by  the  Acomms  software. 

During  Dive  03,  the  vehicle  modem  had  difficulty  decoding  messages  it  received  once 
the  vehicle’s  thrusters  were  enabled  during  the  descent  to  the  bottom  and  while  the  vehicle 
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Vehicle  and  Ship  Trajectory 


Figure  3.6:  The  ship  trackline  and  estimated  vehicle  trackline  during  the  9-hour- long  survey 
at  200m  altitude. 

was  at  depth.  We  believe  this  difficulty  was  due  to  the  location  of  the  modem  transducer  on 
the  lower  hull  where  the  upper  hull  may  have  interfered  with  acoustic  transmissions  from 
the  ship  and/or  due  to  electrical  or  acoustic  noise  from  the  vehicle  thrusters.  The  successful 
throughput  of  messages  was  asymmetric  and  depended  on  the  type  of  message  (32-bit 
mini-packet  versus  full  32-byte  data  packet).  For  the  dive  shown  here,  39%  of  mini  packets 
transmitted  from  the  vehicle  to  the  ship  were  successfully  received  at  the  ship,  whereas 
mini-packets  transmitted  from  the  ship  to  the  vehicle  were  successfully  received  by  the 
vehicle  only  22%  of  the  time.  The  successful  receipt  of  32-byte  data  packets  is  predicated 
on  the  success  of  the  mini-packets:  for  a  32-byte  message  to  be  successfully  received,  a  type 
of  mini-packet  called  a  cycle-init  must  first  be  successfully  received.  Because  the  vehicle 
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was  designated  master,  all  cycle-inits  originated  from  the  vehicle.  Thus,  the  maximum 
percent  of  data  packet  transmissions  possible  is  limited  by  the  39%  success  rate  of  vehicle- 
to-ship  mini-packets.  Of  the  data  transmissions  that  had  successful  cycle-inits,  59%  of  the 
data  packets  transmitted  by  the  vehicle  to  the  ship  were  successfully  received  and  25% 
of  the  ship-to-vehicle  data  packets  were  successfully  received.  This  results  in  an  overall 
success  rate  of  23%  for  vehicle-to-ship  data  packets  and  only  9.5%  for  ship-to-vehicle 
data  packets.  In  addition  to  their  sparsity,  the  messages  received  at  the  vehicle  had  time-of- 
flights  that  were  largely  inconsistent  with  the  the  time-of-flights  of  messages  received  at  the 
ship.  The  cause  of  this  discrepancy  is  still  under  investigation.  The  percentages  presented 
here  are  based  on  a  total  of  ~  2500  mini-packets  (including  ranging  pings  and  cycle-inits) 
and  ~  900  FSK-encoded  data  packets  transmitted  between  the  vehicle  and  the  ship. 

3.4.3  May- June  2009,  Nereus ,  Mariana  Trench 
Synchronous  Three-Node  Communication 

On  May  31,  2009  Nereus  completed  the  first  of  several  dives  to  Challenger  Deep  in  the 
Mariana  Trench,  reaching  a  maximum  depth  of  10,903  m.  During  these  dives  the  Acomms 
software  managed  all  acoustic  communications  between  the  vehicle,  the  depressor,  and  the 
ship.  The  author  was  responsible  for  the  Acomms  system  operations.  This  was  the  first 
multi-node  deployment  with  the  Acomms  system  where  we  made  use  not  only  of  the  syn¬ 
chronized  clocks  on  the  vehicle  and  the  ship  to  predict  the  vehicle’s  acoustic  transmissions, 
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Figure  3.7:  The  science  and  engineering  team  on  board  the  R/V  Kilo  Moana  after  Nereus 
successfully  completed  three  dives  to  Challenger  Deep  in  the  Mariana  Trench.  Photo  credit: 
Barbara  Fletcher,  SPAWAR. 

but  also  actively  initiated  acoustic  messages  at  each  node  as  a  regular  part  of  the  TDMA 
cycle  in  a  true  multi-master  setup.  Figure  3.7  shows  the  entire  science  and  engineering 
team  in  front  of  Nereus  with  a  bathymetric  plot  of  Challenger  Deep. 

As  described  above,  Nereus  has  an  EDO/Straza  SP23  transducer  mounted  at  the  forward 
end  of  the  starboard  hull  facing  upwards.  Operations  during  this  expedition  were  again 
conducted  from  the  R/V  Kilo  Moana  and  the  same  bridle  setup  described  in  Section  3.4.1 
was  used  for  the  shipboard  EDO/Straza  SP23  transducer.  Figure  3.8  shows  the  author  and 
the  marine  tech  on  the  R/V  Kilo  Moana ,  Vic  Polidoro,  deploying  the  shipboard  transducer. 
During  this  expedition,  all  dives  were  carried  out  with  the  vehicle  in  ROV  mode  with  a 
depressor  positioned  around  5800  m  deep  for  the  deep  dives.  The  depressor,  shown  in 
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Figure  3.8:  The  author  and  shipboard  marine  tech  deploying  the  Acomms  transducer  off 
the  stern  of  the  R/V  Kilo  Moana.  Photo  credit:  Catherine  Offinger,  WHOI. 

Figure  3.9,  is  the  transition  point  between  the  standard  steel-armored  cable  from  the  ship 
and  the  micro-fiber  link  to  the  vehicle.  The  depressor  also  carries  a  WHOI  Micro-Modem,  a 
PPSBoard,  and  a  downward  facing  EDO/Straza  SP23  transducer.  The  depressor  modem  is 
controlled  from  the  ship  and  is  intended  to  serve  as  an  intermediate  acoustic  communication 
node  for  more  reliable  communication  with  the  vehicle  at  full  depth.  LBL  transducers  were 
not  deployed  due  to  concerns  over  our  ability  to  trigger  the  acoustic  release  mechanisms 
at  1 1  km  deep  from  the  ship.  A  detailed  description  of  Nereus  and  the  Mariana  Trench 
deployment  is  reported  in  [12]. 

The  six-minute  TDMA  cycle  of  messages  used  during  the  deep  dives,  shown  in  Table 
3.3,  consists  of  three  two-minute  schedules.  Each  schedule  tests  a  different  carrier  fre¬ 
quency  and  bandwidth  combination  (referred  to  as  Bands)  using  three  different  PSK  data 
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Figure  3.9:  The  depressor  shown  on  the  aft  deck  of  the  R/V  Kilo  Moana  with  the  depressor 
design  team  from  SPAWAR.  Photo  credit:  Catherine  Offinger,  WHOI. 


rates  (see  Table  3.4)  and  an  FSK  message  when  applicable  (only  Band  A  supports  FSK). 
Each  two-minute  schedule  includes  a  ~30  second  quiet  time  when  an  operator  could  ini¬ 
tiate  an  acoustic  message  from  the  ship  or  the  depressor,  such  as  an  Abort  or  a  Ranging 
Ping.  Sandipa  Singh  was  responsible  for  the  design  of  the  TDMA  cycle,  while  the  author 
was  responsible  for  its  implementation  and  Acomms  operations  during  the  expedition.  Be¬ 
cause  switching  carrier  frequencies  and  bandwidths  requires  changing  the  configuration  on 
both  the  sending  and  receiving  modems,  this  TDMA  cycle  would  be  impossible  in  practice 
without  synchronized  clocks  on  the  vehicle,  the  depressor,  and  the  ship. 

As  shown  in  Table  3.4.3  the  successful  throughput  of  messages  versus  range  varied  by 
message  types.  Note  that  the  carrier  frequency  is  given  for  reference  only — the  acoustic 
range  is  not  directly  correlated  to  the  carrier  frequency.  The  operational  range  of  the  lower 
data-rate  PSK  messages,  11km,  far  exceeded  the  team’s  expectations,  allowing  direct  com- 
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Table  3.3:  Typical  TDMA  cycle  of  modem  messages  during  Challenger  Deep  dives. 


Command 

Description 

Time  [s] 

Configs 

Band  A  (10  kHz,  5kHz  bandwidth) 

Set  band,  carrier  freq,  bandwidth 

9 

Ping 

Topside  pings  Vehicle 

21 

Data  Broadcast 

Vehicle  sends  state  data  (FSK,0) 

15 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,2) 

15 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,4) 

15 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,5) 

15 

Ping 

Depressor  pings  Vehicle 

20 

Pause 

Until  the  top  of  even  minute 

10 

Configs 

Band  0  (12  kHz,  2kHz  bandwidth) 

Set  band,  carrier  freq,  bandwidth 

9 

Ping 

Topside  pings  Vehicle 

21 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,2) 

15 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,4) 

15 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,5) 

15 

Ping 

Depressor  pings  Vehicle 

20 

Pause 

Until  the  top  of  even  minute 

25 

Configs 

Band  0  (8  kHz,  1.25kHz  bandwidth) 

Set  band,  carrier  freq,  bandwidth 

9 

Ping 

Topside  pings  Vehicle 

21 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,2) 

15 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,4) 

15 

Data  Broadcast 

Vehicle  sends  state  data  (PSK,5) 

15 

Ping 

Depressor  pings  Vehicle 

20 

Pause 

Until  the  top  of  even  minute 

25 

Total  TDMA  Cycle  Time  [s]  = 

360 

munication  from  the  vehicle  to  the  ship  throughout  the  deep  dives.  A  complete  analysis  of 
acoustic  communication  performance  during  this  expedition  is  reported  in  [84]. 
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Table  3.4:  Rate  table  for  the  WHOI  Micro-Modem 


Rate 

Max  payload 
[bytes] 

Burst  rate  [bps]  per  Bandwidth 

5000  Hz  2000  Hz  1250  Hz 

0 

32 

65 

n/a 

n/a 

2 

192 

520 

208 

130 

4 

512 

1300 

520 

325 

5 

2048 

5380 

2150 

1340 

Table  3.5:  Approximate  range  for  various  PSK  rates  and  band¬ 
width  combinations  through  the  vertical  acoustic  channel. 


PSK  rate 

Range  @  Bandwidth  (Carrier  Frequency) 

1 ,25kHz  (8kHz)  2kHz  ( 12kHz)  5kHz  ( 10kHz) 

2 

9  km 

11  km 

7  km 

4 

9  km 

10  km 

6  km 

5 

5  km 

6  km 

4  km 

3.4.4  October  2009,  Nereus ,  Cayman  Trough 
On-The-Fly  Subsea  Mission  Corrections 


In  October  2009  the  Acomms  system  was  again  deployed  with  Nereus  to  the  Cayman 
Trough  in  the  western  Caribbean  Sea.  The  mission  of  this  expedition  was  to  locate  and 
map  new  hydrothermal  vents  along  the  Mid-Cayman  Rise,  an  ultra-slow  spreading  center 
with  a  maximum  depth  just  over  6,800  m  located  at  the  center  of  the  trough  [7],  During 
this  expedition  Louis  L.  Whitcomb  and  James  C.  Kinsey  were  responsible  for  the  Acomms 
system  operations.  During  the  first  leg  of  the  expedition  Nereus  was  configured  as  an  AUV; 
during  the  second  leg  of  the  expedition  Nereus  was  operated  as  an  ROV.  The  survey  area 
in  both  cases  was  around  5200  m  deep.  During  AUV  operations,  the  vehicle  operators  used 
the  Acomms  system  to  monitor,  in  real  time,  both  the  vehicle  position  and  health  through 
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navigation  data  packet  uplinks  and  the  science  sensor  data  through  science  data  packet  up¬ 
links.  In  addition,  acoustic  transmissions  to  the  vehicle  were  used  to  modify  the  vehicle 
mission  plan  in  real  time.  With  this  capability,  the  vehicle’s  path  could  be  reprogrammed 
from  the  surface  without  recovering  and  re-launching  the  vehicle — a  lengthy  process  when 
the  vehicle  is  operating  at  5200  m.  Figure  3.10  shows  the  science  data  that  was  acoustically 
sent  to  the  ship  from  the  vehicle,  allowing  the  science  and  operations  team  to  monitor  the 
vehicle’s  progress  in  real  time.  Part  way  through  the  survey,  the  operations  team  acousti¬ 
cally  broadcast  new  trackline  information  causing  the  vehicle  to  modify  its  mission  plan 
in  order  to  survey  an  area  to  the  west  of  the  original  survey  area.  The  new  survey  area 
contained  strong  evidence  of  hydrothermal  activity  as  shown  in  the  science  data  broadcast 
from  the  vehicle  [37].  This  functionality,  not  unique  to  the  Acomms  system,  underscores 
the  potential  for  acoustic  communication  to  revolutionize  autonomous  vehicle  operations. 

3.5  Chapter  Summary 

The  Acomms  system  has  proven  to  be  a  valuable  asset  during  autonomous  vehicle  op¬ 
erations,  allowing  AUV  operators  to  monitor  the  status  of  the  vehicle  in  real  time,  enabling 
multi-node  systems  to  communicate  efficiently  without  packet  collision,  and  enabling  on- 
the-fly  mission  corrections  to  the  vehicle  during  subsea  operations.  In  addition,  one-way 
travel-time  data  collected  by  the  author  and  collaborators  using  the  Acomms  system  has 
experimentally  validated  single-beacon  navigation  in  post-processing. 
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Figure  3.10:  The  vehicle  track  over  the  Mid-Cayman  Rise  with  acoustically  uplinked  sci¬ 
ence  data  collected  by  the  vehicle.  The  xy-axes  are  in  meters  and  the  color  of  the  data  points 
represents  redox  potential  (Eh),  the  strength  of  the  chemical  signature  in  the  water  from  the 
hydrothermal  vent,  where  blue  is  strongest.  The  vehicle  mission  was  reprogrammed  acous¬ 
tically  from  the  surface  to  move  the  programmed  survey  to  the  west  of  the  original  survey 
as  shown.  Photo  credit:  WHOI. 
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System  Models 


This  chapter  describes  the  process  models  for  the  vehicle  and  the  ship  and  the  observa¬ 
tion  models  used  in  the  formulation  of  both  the  Kalman  filter  and  the  information  filter. 


4.1  State  Description 


The  complete  state  vector  of  the  system,  denoted  with  a  different  font  x,  consists  of 
the  current  vehicle  estimate,  xv,  the  current  ship  estimate,  xs,  and  a  fixed-length  queue 
of  historic  states  representing  the  ship  and  vehicle  position  at  the  beginning  of  the  second 
(referred  to  as  the  top  of  the  second)  for  the  most  recent  n  seconds,  denoted  xv_t  and 


for  i  e  [1, n\. 


x  = 


T  „T 


[xv,x 
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v—  1  ? 
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s—  li 


••  x r 
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The  current  vehicle  state  contains  local-level  pose  and  attitude,  as  well  as  body-frame  linear 
and  angular  velocities 

xv  =  [sT,vT  ,vT,uT]T  (4.2) 


X 

0 

u 

V 

y 

,  V  = 

9 

,  v  = 

V 

,  U )  = 

Q 

z 

.  ^  . 

w 

r 

where  s  is  the  local-level  vehicle  pose  in  the  local  frame,  tp  is  the  local-level  vehicle  attitude 
(Euler  roll,  pitch,  heading),  v  is  the  body-frame  linear  velocity,  and  cj  is  the  body-frame 
angular  velocity.  The  current  ship  state  contains  x-y  position,  heading,  and  the  respective 
velocities 

xs  =  [xs,  ys,  Qs,  xs,  ys,  0S]T.  (4.4) 

The  historic  states  contain  full  estimates  of  the  vehicle  state  and  the  ship  state  from 
previous  time  steps.  Historic  states  are  necessary  for  causal  processing  of  range  measure¬ 
ments  because  of  the  time  required  for  an  acoustic  data  packet  to  propagate  from  the  sender 
to  the  receiver.  When  the  acoustic  modems  are  in  synchronous  navigation  mode  (see  Sec¬ 
tion  3.3.1)  all  acoustic  transmissions  are  initiated  at  the  top  of  the  second.  Thus,  in  order  to 
ensure  that  the  state  vector  contains  the  appropriate  historic  states  needed  to  perform  range 
measurement  updates,  the  CEKF  maintains  an  estimate  of  the  state  of  the  system  at  the  top 
of  the  second  for  the  previous  n  seconds.  In  practice  n  =  6  for  this  implementation,  which 
enables  the  algorithm  to  accommodate  range  measurements  with  travel  times  of  up  to  six 
seconds  (i.e.  9000  m  range  assuming  1500  m/s  sound  velocity). 
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4.2  Vehicle  Process  Model 


We  seek  a  navigation  algorithm  that  is  independent  of  the  vehicles  on  which  it  is  em¬ 
ployed.  Thus,  as  is  common  for  navigation  algorithms,  we  use  a  constant-velocity  kine¬ 
matic  model  of  the  system  that  does  not  incorporate  vehicle  dynamics,  drag  models,  or 
thrust  or  steering  inputs.  The  constant- velocity  assumption  is  valid  in  the  context  of  un¬ 
derwater  vehicles  for  several  reasons.  An  autonomous  vehicle  survey  typically  consist  of 
a  “mowing-the-lawn”  vehicle  track  consisting  of  long,  straight,  constant-velocity  segments 
with  short  turns  in  between.  In  addition  the  vehicle  is  subject  to  low  bandwidth  control 
and  moves  slowly  relative  to  the  precision  and  frequency  of  measurement  updates.  During 
stops,  starts,  and  turns  the  vehicle  accrues  bounded  error,  but  the  estimation  error  converges 
once  the  vehicle  has  returned  to  constant- velocity  motion,  as  has  been  validated  by  [33]. 

The  following  formulation  of  the  process  model  and  the  details  of  its  linearization  and 
discretization  presented  here  are  from  [25].  As  stated  above,  we  use  a  constant- velocity 
process  model  for  the  vehicle 
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where  R(tp)  is  the  transformation  from  body-frame  to  local-level  linear  velocities,  3 \<p)  is 
the  transformation  from  body-frame  angular  velocities  to  Euler  rates,  and  wv  ~  Af(0,  Q ,,) 
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is  the  independent  zero-mean  Gaussian  process  noise  in  the  acceleration  term.  R((p)  and 


J(^p)  are  found  by  solving 


R(V)=RT4,RjRj  (4.6) 
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J  = 


1  sin  0  tan  9  cos  0  tan  6 
0  cos  0  —  sin  0 

0  sin  0  sec  9  cos  0  sec  9 


(4.8) 


Linearization:  We  linearize  the  vehicle  process  model  (4.5)  about  fit,  our  estimate  of  the 
state  at  time  t,  using  the  Taylor  series  expansion 


xv(t )  =  f(i ut)  +  Fx(xv(t )  -  nt)  +  HOT  +  Gvwv(t )  (4.9) 


where 


Fx  = 


df{xz 


dx. 


xv{t)=nt 


(4.10) 


55 


CHAPTER  4.  SYSTEM  MODELS 

and  HOT  denotes  higher  order  terms.  Dropping  the  HOT  and  rearranging  we  get 


xv(t)  S3  Fxxv(t )  +  -  Fx/j,k  +Gvwv(t )  (4.11) 

" - V - ' 

u(t) 

=  Fxxv(t )  +  u(t)  +  Gvwv(t)  (4.12) 

where  f(nt)  —  Fx^it  is  treated  as  a  constant  input  pseudo  control  u{t). 

Discretization:  In  order  to  find  a  discrete-time  model  of  the  linearized  vehicle  process 
model  we  rewrite  (4.12)  as 


•^(t)  Fxxv(t)  T  Bvu(t)  T  Gvwv(t) 


(4.13) 


where  Bv  =  I.  Assuming  zero-order  hold  and  using  the  standard  method  [8]  to  discretize 
over  a  time  step  T  we  solve  for  FVk  and  BV)  in  the  discrete  form  of  the  process  model: 


XVk+ 1 


F Vk Xvk  BVkUk  ~\~  'Mvk 


(4.14) 


FVk  e 
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'0 


,Fx(T-r)BjT 

£Fx  H—t)  df 

rT 


=  e 


FXT 


„  —  Fxr 


dr 


(4.15) 


(4.16) 
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for  Fx  nonsingular.  The  discretized  process  noise  wVk  has  the  form 


Jo 


for  which  we  can  calculate  the  mean  and  variance  as  follows: 


E  [wVk] 


=  0 


(4.17) 


(4.18) 


where  we  make  use  of  the  facts  that  the  expected  value  can  be  brought  inside  the  integral 
because  it  is  a  linear  operator  and  the  noise  vector  wv  is  independent  in  time  so  that  the 
covariance  E  [ wv(t)w J (7)]  is  zero  except  when  7  =  r. 
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4.3  Ship  Process  Model 

Similar  to  the  vehicle,  we  use  a  constant-velocity  process  model  for  the  ship.  As  de¬ 
scribed  for  the  vehicle  process  model,  this  is  a  valid  assumption  because  of  the  typical  ship 
operations,  holding  station  or  steaming  at  a  constant  speed,  and  the  relative  speed  of  the 
ship  compared  to  the  precision  and  frequency  of  GPS  and  gyro  measurement  updates. 


xa  = 


0  I 
0  0 


x. 


0 

I 


UK 


G's 


(4.20) 


where  ws  ~  A/"(0,  Qs)  is  the  independent  zero-mean  Gaussian  process  noise  in  the  accel¬ 
eration  term.  The  ship  process  model  (4.20)  is  already  linear  and  does  not  require  lineariza¬ 
tion.  It  is  discretized  in  the  same  fashion  as  the  vehicle  model: 


xsk+1  =  FSkxSk  +  wSk  (4.21) 

FSk  =  eF°T  (4.22) 


where  the  higher  order  terms  are  identically  zero  because  of  the  structure  of  Fs,  resulting 
in  a  simple  closed-form  solution  for  FSk.  Note  that  BSk  =  0  because  Bs  =  0.  The 
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discretized  ship  process  noise 

eFs^T~T^Gsws(T)dT,  (4.23) 


can  also  be  shown  to  be  zero-mean  Gaussian  using  formulas  (4.18)  and  (4.19),  such  that 
wSk  ~  A/"(0,  QSk ).  Due  to  the  structure  of  FSk,  the  covariance  matrix  simplifies  to 
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Sk 


3 

lji'2 

21 


Q 


(4.24) 


4.4  Observation  Model 

The  range  measurement  from  the  ship’s  modem  to  the  vehicle’s  modem  is  a  nonlinear 
function  of  the  current  vehicle  state  and  an  historic  ship  state.  For  simplicity  of  notation, 
we  assume  here  that  the  modems  are  located  at  the  origin  of  their  respective  local  frames 
and  that  the  ship’s  modem  has  a  depth  of  0  m. 

The  measurement  equation  for  a  range  measurement  made  from  an  acoustic  data  packet 
sent  from  the  ship  to  the  vehicle  is 


z 


mg 


X, 


\T  i 


Xn 


X, 


J  +V; 


rng 


(4.25) 


where  xSxyz  is  the  ship  pose  at  the  time  of  launch  of  the  acoustic  data  packet,  tTOL,  and 
xVxyz  is  the  vehicle  pose  at  the  time  of  arrival  of  the  acoustic  data  packet,  tTOA.  We  assume 
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zero-mean  Gaussian  measurement  noise,  vrng  ~  J\f( 0,  Rrng),  which  is  in  units  of  distance 
and  represents  the  imprecision  in  timing  multiplied  by  the  speed  of  sound.  The  covariance 
Rrng  is  assumed  to  be  identical  for  all  range  measurements  and  therefore  does  not  have  the 
time-dependent  subscript  k.  We  can  rewrite  (4.25)  in  terms  of  the  state  vector  as 

Zrng  =  {xJ  MJ  Mx)^  +  Vrng  (4.26) 


where 


M 


0-0  Js 


(4.27) 


and  Jv  and  Js  are  defined  to  capture  the  pose  information  of  the  vehicle  and  ship1 
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(4.31) 


assuming  that  tTOA  is  the  current  time  and  tTOL  is  the  the  top  of  the  second  when  the  acoustic 


signal  was  broadcast  from  the  ship. 

1  Jv  and  Js  are  defined  explicitly  as 
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(4.28) 

(4.29) 
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The  Jacobian  of  the  measurement  with  respect  to  x,  Hrngk,  is 


H 


9zrng  (x) 


rngk 


dx 


x— Mfc|fc-i 
T 


-  (Mfclfc-l-^  '  -^Mfclfc-l)  2  ^k\k-l^T  M ■ 


(4.32) 


Measurements  from  additional  navigation  sensors,  e.g.  depth  sensor,  gyrocompass,  and 
Doppler  velocity  log,  are  processed  asynchronously  using  standard  observation  models. 
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Chapter  5 

Extended  Kalman  Filter  for 
Single-Beacon  Navigation 

5.1  Introduction 

This  chapter  describes  the  formulation  and  implementation  of  a  centralized  extended 
Kalman  filter  (CEKF)  designed  to  estimate  vehicle  and  ship  position  within  the  framework 
of  one-way-travel-time  (OWTT)  navigation.  The  experimental  results  presented  demon¬ 
strate  that  single-beacon  navigation  is  a  viable  alternative  to  traditional  absolute  navigation 
methods  such  as  long  baseline  (LBL)  and  ultra-short  baseline  (USBL)  navigation  (dis¬ 
cussed  in  detail  in  Chapter  2).  The  CEKF  is  applicable  in  post-processing,  where  sensor 
measurements  from  both  the  ship  and  the  vehicle  are  available  simultaneously.  The  CEKF 
is  used  in  Chapter  6  as  a  benchmark  for  the  decentralized  algorithm.  The  implementation 
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described  here  is  for  a  single  surface  node,  the  ship,  and  a  single  underwater  vehicle,  though 
the  algorithm  is  trivially  extensible  to  multiple  vehicles. 

This  chapter  describes  the  formulation  of  the  extended  Kalman  filter  designed  by  the 
author  and  collaborators  for  OWTT  navigation  and  reports  field  results  using  data  collected 
by  the  author  and  collaborators  from  an  autonomous  underwater  vehicle  (AUV)  survey 
carried  out  in  4000  m  of  water  on  the  southern  Mid- Atlantic  Ridge  [93]. 

5.2  Centralized  Extended  Kalman  Filter 

An  extended  Kalman  filter  is  employed  to  fuse  depth,  gyrocompass,  and  Doppler  veloc¬ 
ity  measurements  from  the  vehicle,  position  and  attitude  measurements  from  the  ship,  and 
range  measurements  between  the  vehicle  and  the  ship.  The  CEKF  is  designed  to  estimate 
the  current  and  previous  states  of  both  the  ship  and  the  vehicle  and  is  applicable  in  post¬ 
processing  of  previously  acquired  dive  data.  This  section  briefly  reviews  the  formulation 
of  the  extended  Kalman  filter  followed  by  the  details  of  the  centralized  implementation  for 
OWTT  navigation,  summarized  in  Algorithm  1.  A  derivation  of  the  linear  Kalman  filter  is 
provided  in  Appendix  B . 

5.2.1  Review  of  EKF  Formulation 

The  extended  Kalman  filter  is  a  sub-optimal  filter  that  recursively  estimates  system 
state.  The  EKF  applies  the  general  approach  of  the  Kalman  filter  [50],  the  optimal  linear 
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Algorithm  1  CEKF  with  state  augmentation 
1 :  loop  {perform  prediction  and  measurement  update} 

2:  At  =  :mm(time  until  top  of  the  second;  time  until  next  measurement;  0.1s) 

3:  if  current  time  (before  prediction)  is  the  top  of  the  second  then 

4:  augment  state  vector  with  prediction  of  current  state  forward  At  (5. 12,  5.13) 

5:  else 

6:  predict  current  state  forward  At  (5.15,  5. 16) 

7:  end  if 

8:  if  3  measurements  at  this  time  step  then 

9:  perform  measurement  update  (5.8,  5.9) 

10:  end  if 

1 1 :  end  loop 


estimator,  to  nonlinear  plants  by  linearizing  the  plant  process  and  observation  models  along 
the  trajectory  of  the  system.  The  formulation  reported  here  is  for  a  nonlinear  plant  with 
discrete  observations  [36].  Consider  the  general  nonlinear  plant  process  and  observation 
model 


x{t)  =  f(x(t),t)  +  G(t)w(t)  (5.1) 

zk  =  h(x(tk))  +  vk,k  =  1,2,  •••  (5.2) 

where  x(t)  is  the  state  in  continuous  time,  w(t)  ~  A/"(0,  Q(t))  is  the  independent  zero- 
mean  Gaussian  process  noise,  zk  is  the  measurement  at  time  step  tk  in  discrete  time,  and 
vj,  ~  A/”(0,  Rk)  is  the  independent  zero-mean  Gaussian  measurement  noise.  Note  that 
boldface  math  symbols,  e.g.  x  and  Q,  represent  vectors  and  matrices,  while  standard 
weight  math  symbols,  e.g.  t  and  k,  represent  scalars. 
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The  CEKF  reported  herein  employs  a  discrete-time  linearization  of  the  process  model 


xk+1  =  Fkxk  +  Bkuk  +  wk  (5.3) 


to  recursively  estimates  the  mean,  /r,  and  covariance,  S,  of  the  state  vector  x 


/j,  —  E[x\ 

(5.4) 

F 

"5 

1 

3^ 

1 

W 

(5.5) 

resulting  in  process  prediction  equations 


AC+1#  —  ^kFk\k  4"  BkUk 

(5.6) 

Sfe+i|fe  =  FkTik\kFj  +  Qk 

(5.7) 

where  Fk  is  the  discrete-time  linear  state  transition  matrix,  Bk  is  the  discrete-time  linear 
input  matrix,  Qk  is  the  discrete-time  process  noise  covariance,  uk  is  the  piecewise-constant 
input  at  time  step  tk,  and  we  use  T  as  the  transpose  operator. 

The  measurement  update  equations  for  the  extended  Kalman  filter  are 

AC#  =  AC#-i  +  Kk(zk  -  h{nk ifc-0)  (5.8) 

^ k\k  1  -K  kFI  k^Jk  |fc— i  (5.9) 
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where  Hk  is  the  Jacobian  of  h  at  time  step  tk 


Hk 


dh(x(tk)) 


dx(tk) 


x{tk)  —  ^k\k  —  l 


(5.10) 


and  Kk  is  the  Kalman  gain  at  time  step  tk,  given  by 


Kk  —  '£‘k\k-iH~l(HkHk\k_iH~[  +  Rk)  1.  (5.11) 


5.2.2  Process  Prediction  and  Augmentation 

The  complete  state  process  prediction  for  the  EKE  is  written  in  terms  of  the  full  state 
vector  of  the  system  defined  in  (4.1).  Combining  the  discrete-time  linearized  vehicle  and 
ship  process  models  (4.14)  and  (4.21),  and  substituting  them  into  the  discrete-time  lin¬ 
earized  Kalman  process  prediction  equation  (5.6),  the  complete  state  process  prediction 
becomes 
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(5.12) 


(5.13) 
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where 


Qk 


QVk  o  o  ■  ■  •  o 
0  QSk  o  ■  ■  •  o 
0  0  0  0 


0  0  0  0 


(5.14) 


Note  that  the  historic  states  do  not  change  during  this  process  update. 

A  modified  process  prediction  is  necessary  at  the  top  of  the  second  when  state  augmen¬ 
tation  is  done  in  concert  with  the  process  prediction.  During  this  modified  prediction  step, 
in  addition  to  predicting  forward  the  current  vehicle  state,  the  estimate  of  the  current  state 
(before  the  prediction)  is  added  to  the  state  vector  while  simultaneously  marginalizing  out 
the  oldest  historic  state,  i.e.  (x„_n,  xs_n) 
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(5.15) 


(5.16) 


and  Qk  is  as  defined  before. 
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5.2.3  Measurement  Update 

We  find  the  extended  Kalman  filter  measurement  update  equations  for  a  range  measure¬ 
ment  by  substituting  the  Jacobian  of  the  range  measurement  (4.32)  into  (5.8)  and  (5.11) 

=  Krngk  (zrngk  ~  (^k\k-l^  (5-17) 

Krngk  =  '^k\k-lHrngk(Hrngk'Ek\i<._iHrngk  -\~  Rrng)  .  (5.18) 


5.3  Field  Results 

Sea  trials  were  conducted  by  the  author  and  collaborators  using  the  Acomms  system 
with  WHOI  Micro-Modems  and  PPSBoards  during  an  expedition  on  the  R/V  Knorr  to  the 
southern  Mid-Atlantic  Ridge  in  January  2008.  The  goal  of  the  expedition  was  to  test  and 
evaluate  engineering  methods  for  locating  and  mapping  new  hydrothermal  vents  on  the 
southern  Mid-Atlantic  Ridge.  Navigation  data  collected  by  the  author  and  collaborators 
during  this  expedition  is  used  to  experimentally  validate  the  CEKF  in  post-processing. 
These  results  are  reported  here  and  were  published  in  [93]. 

5.3.1  Site  Description 

The  southern  Mid- Atlantic  Ridge  (SMAR)  is  a  divergent  boundary  between  the  South 
American  Plate  and  the  African  Plate  that  is  presently  spreading  at  about  2.5  cm  per  year. 
The  survey  site,  shown  in  Figure  5.1,  is  located  near  04°  S  12°  W  in  a  deep  non-transform 
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discontinuity  whose  maximum  depth  exceeds  4000  m  [38].  Operations  were  conducted  on 
a  section  of  the  SMAR  to  the  north  of  the  sites  where  active  hydrothermal  vents  were  first 
discovered  by  a  combination  of  deep-tow  and  deep-submergence  technologies  culminating 
in  photography  by  ABE  [38]  and  subsequently  sampled  by  the  ROV  Marum  Quest  [40]. 

5.3.2  Experimental  Setup 

The  data  presented  here  were  collected  by  the  author  and  collaborators  using  the  au¬ 
tonomous  underwater  vehicle  (AUV)  Puma,  developed  at  Woods  Hole  Oceanographic  In¬ 
stitution  [82].  Puma  is  a  5000  m  rated  AUV  equipped  with  the  following  navigation  sen¬ 
sors:  a  Paroscientific  pressure  depth  sensor,  an  OCTANS  fiber-optic  gyrocompass  for  atti¬ 
tude  and  attitude  rate  measurements,  and  a  300  kHz  RDI  Doppler  velocity  log  (DVL)  for 
velocity  measurements.  The  vehicle  is  also  equipped  with  a  WHOI  Micro-Modem  [31]  and 
an  ITC-3013  transducer  [45]  for  acoustic  communications  and  range  measurements  and  a 
PPSBoard  for  precision  timing  [27,28].  The  Micro-Modem  and  PPSBoard  are  described  in 
more  detail  in  Chapter  3.  The  author  was  responsible  for  the  development  and  operations 
of  the  Acomms  system  during  this  expedition.  Chris  Murphy  assisted  with  the  integration 
of  the  Acomms  software  on  Puma. 

The  R/V  Knorr  is  a  279  ft  oceanographic  research  vessel  operated  by  WHOI.  The  ship 
has  two  azimuthing  stern  thrusters,  a  retractable  azimuthing  bow  thruster,  and  dynamic 
positioning  (DP)  capability  enabling  it  to  hold  station  and  maneuver  in  any  direction  [97]. 
For  the  ship’s  position  information  we  used  the  C-Nav  2000  Real-Time  GIPSY  (RTG)  GPS 
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Figure  5.1:  (a)  R/V  Knorr  (b)  AUV  Puma  (c)  The  survey  site  is  shown  by  the  red  box  near 
Ascension  Island  on  the  southern  Mid- Atlantic  Ridge.  Photo  credits:  WHOI. 

with  a  reported  horizontal  accuracy  of  10  cm  [19].  An  Applanix  POS/MV-320  provided 
heading,  pitch,  and  roll  data  with  a  reported  accuracy  of  0.02°  [2].  The  ship  was  also 
equipped  with  a  WHOI  Micro-Modem  [31]  and  an  ITC-3013  transducer  for  sending  and 
receiving  acoustic  data  packets.  Figure  5.1  shows  the  R/V  Knorr,  the  AUV  Puma  and  the 
survey  area  in  the  red  box  near  Ascension  Island. 

On  Puma  Dive  03,  the  vehicle  conducted  a  survey  comprised  of  12  tracklines  approxi¬ 
mately  65  meters  apart  and  700  meters  long  while  maintaining  an  altitude  of  200  m.  While 
the  vehicle  carried  out  the  survey  mission,  we  repositioned  the  R/V  Knorr  around  the  sur¬ 
vey  site  in  a  diamond  shaped  pattern,  holding  station  at  each  apex.  This  was  done  to  pro- 
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vide  range  measurements  to  the  vehicle  from  different  locations  for  increased  observability. 
During  these  field  trials  we  used  two-way  acoustic  communication  between  the  vehicle  and 
the  ship  initiated  by  the  vehicle.  Acoustic  data  packets  were  sent  from  the  vehicle  to  the 
ship  and  requested  by  the  vehicle  from  the  ship  every  30  seconds. 

5.3.3  Initialization 

Because  the  EKF  algorithm  performs  linearization  along  the  system  trajectories,  an 
initial  state  estimate  too  far  from  the  actual  state  could  cause  the  algorithm  to  be  unstable. 
In  this  implementation  we  initialize  the  CEKF  with  a  maximum-likelihood  estimate  (MLE) 
of  the  vehicle  state  and  covariance.  For  this  implementation  of  the  CEKF,  the  maximum- 
likelihood  estimation  is  performed  over  the  entire  data  set  as  previously  reported  in  [28]. 

5.3.4  Sensor  Offsets 

The  vehicle  reference  frame  is  defined  to  be  coincident  with  the  Doppler  frame.  Any 
angular  offset  between  the  OCTANS  and  the  Doppler  is  accounted  for  as  a  mounting  offset 
in  the  OCTANS.  Doppler  attitude  measurements  were  not  used  by  the  CEKF,  but,  by  com¬ 
paring  them  to  the  OCTANS  pitch  and  roll  measurements  in  post-processing,  we  calculated 
the  angular  offset  of  the  OCTANS  to  be  -3.24°  and  0.64°  in  pitch  and  roll  respectively.  The 
OCTANS  heading  offset  was  estimated  by  analyzing  the  mean  and  standard  deviation  of 
the  error  between  the  EKF-estimated  vehicle  position  and  the  LBL  vehicle  position  over 
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the  entire  trackline  for  various  OCTANS  heading  offsets,  shown  in  Figure  5.2,  assuming 
the  previously  stated  roll  and  pitch  offsets.  Given  these  data  a  3.5°  heading  offset  in  the 
gyrocompass  was  assumed  and  accounted  for  as  a  mounting  offset  in  the  OCTANS. 

Error  between  LBL  and  EKF  vehicle  position 
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Figure  5.2:  East- West  and  North-South  components  of  the  error  between  the  EKF- 
estimated  vehicle  position  and  the  LBL  vehicle  position.  Errors  are  calculated  over  the 
entire  trackline  for  a  range  of  OCTANS  heading  offsets. 


5.3.5  Results 


The  integrity  of  the  vertical  acoustic  telemetry  channel  varied  over  the  course  of  the 
dive.  While  the  vehicle  was  surveying  near  the  bottom,  on  average  one  acoustic  data  packet 
from  which  we  could  calculate  range  was  successfully  received  every  90  seconds.  Figure 
5.3  shows  an  XY  plot  of  the  vehicle  trajectory  as  estimated  by  the  CEKF  compared  with  the 
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Vehicle  and  Ship  Trajectory 


Figure  5.3:  EKF  estimate  of  vehicle  position  compared  to  LBL  fixes  with  an  overlay  of  the 
ship’s  track. 

vehicle  position  fixes  from  LBL.  LBL  fixes  were  largely  unavailable  on  tracklines  where 
the  vehicle  was  heading  east,  most  likely  due  to  shadowing  of  the  transducer  by  the  vehicle 
frame  at  this  vehicle  heading.  Calculating  the  difference  between  the  LBL  fixes  and  the 
corresponding  CEKF  estimated  vehicle  position  in  the  East- West  direction  and  the  North- 
South  direction,  we  find  that  error  in  the  East-West  direction  has  a  mean  of  -10.3m  and  a 
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standard  deviation  of  10.2  m,  while  the  error  in  the  North-South  direction  has  a  mean  of 
-13.5  m  and  also  a  standard  deviation  of  10.2  m.  These  errors  compare  favorably  with  LBL, 
which  has  1-10  m  typical  accuracy.  However,  the  non-zero  mean  suggests  the  presence  of 
systematic  errors  that  are  not  accounted  for  in  the  reported  sensor  calibrations.  The  author 
and  collaborators  are  currently  pursuing  a  more  rigorous  evaluation  of  sensor  calibration. 

5.3.6  Sources  of  Error 

Unmodelled  non-zero-mean  or  non-Gaussian  noise  violates  the  assumptions  of  the 
Kalman  filter  and  is  a  source  of  error  in  the  filter’s  estimate.  In  addition  to  sensor  off¬ 
sets  discussed  above,  other  possible  sources  of  error  in  this  experiment  are  the  estimate  of 
the  LBL  beacon  positions  and  the  assumption  of  a  constant  sound  velocity  profile. 

LBL  Accuracy:  While  submerged,  the  vehicle  used  range  information  in  the  form  of  two- 
way  travel  times  from  three  LBL  beacons  to  estimate  its  absolute  position  in  real  time  [44]. 
The  accuracy  of  the  vehicle  position  estimate  from  LBL  ranges  is  predicated  on  the  accu¬ 
racy  to  which  the  position  of  the  LBL  beacons  is  known — uncertainty  in  beacon  location 
translates  directly  to  uncertainty  in  the  vehicle  position  estimate  in  the  radial  direction  from 
the  beacon.  The  LBL  beacon  survey  on  this  expedition,  directed  by  Michael  Jakuba,  used 
the  standard  procedure  of  collecting  two-way  travel  times  from  the  ship  to  the  individual 
beacons  from  5-10  different  ship  locations  after  each  beacon  reaches  the  seafloor.  The  ship 
locations  are  spaced  approximately  equally  around  a  circle  with  ~lkm  horizontal  radius 
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from  each  beacon’s  estimated  bottom  location.  Beacon  location  is  then  estimated  using 
a  least-squares  algorithm  after  outliers  have  been  manually  rejected.  Table  5.1  shows  the 
position  of  the  three  LBL  beacons  used  for  this  survey  relative  to  the  survey  site  and  the 
residual  root  mean  squared  error  of  the  estimated  beacon  position  when  surveyed  in  as 
described. 


Table  5.1:  LBL  Beacon  location  and  accuracy  of  position  estimate. 


Beacon 

Approx.  Location 

RMS  Error 

A 

3  km  West 

1.8  m 

B 

3  km  North 

3.8  m 

C 

2.5  km  East 

3.7  m 

Sound  Velocity  Estimation:  In  this  implementation,  the  CEKF  uses  a  depth-weighted 
average  sound  velocity  to  calculate  range  from  the  travel  time  of  acoustic  data  packets.  The 
actual  sound  velocity  profile,  however,  varies  over  depth  as  shown  in  Figure  5.4.  Refrac¬ 
tion  due  to  the  change  in  sound  speed  can  cause  ray  bending  in  acoustic  signals  transmitted 
through  the  water  column.  As  a  result,  the  travel  time  of  an  acoustic  signal  is  not  directly 
proportional  to  slant  range  and  is  dependent  on  the  horizontal  displacement  between  the 
vehicle  and  the  ship. 


To  quantify  this  error,  we  consider  a  range  estimate  between  the  vehicle  and  the  ship 
when  the  ship  is  at  the  western-most  apex  of  its  diamond  pattern  and  the  vehicle  is  at  the  far 
eastern  edge  of  its  survey,  thus  incorporating  the  largest  horizontal  offset  possible  (1236  m 
horizontal  offset).  Calculating  the  difference  in  the  range  estimate  found  using  ray-bending 
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Sound  Velocity  Profile 


Figure  5.4:  Sound  velocity  computed  from  data  from  the  conductivity-temperature-depth 
(CTD)  sensor  on  Puma. 

techniques  [88]  versus  the  depth-weighted  average  sound  velocity,  we  find  that  assuming 
a  constant  sound  velocity  equal  to  the  depth-weighted  average  incurs  an  error  in  the  range 
estimate  on  the  order  of  one  meter  and  therefore  is  not  a  substantial  source  of  error  in  this 
data  set. 
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5.4  Chapter  Summary 

This  chapter  reports  results  from  deep-water  sea  trials  evaluating  a  single -beacon  one- 
way-travel-time  navigation  implemented  with  a  centralized  EKF.  Results  from  the  CEKF 
are  compared  to  the  ground  truth  absolute-navigation  from  LBL  position  fixes.  The  dif¬ 
ference  between  the  CEKF  results  and  the  LBL  fixes  is  commensurate  with  the  errors  we 
typically  expect  from  LBL,  leading  us  to  conclude  that  single -beacon  navigation  is  a  viable 
alternative  to  LBL  navigation  for  deep-water  applications  where  the  ship  or  surface  node 
can  be  moved  around  the  survey  site  to  provide  appropriate  geometric  constraints  on  the 
vehicle  position  estimate. 
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Chapter  6 

Extended  Information  Filter  for 
Single-Beacon  Navigation 

6.1  Introduction 

The  information  filter,  originally  conceived  as  a  decentralized  formulation  of  the  Kalman 
filter  [50],  is  a  recursive  estimator  for  linear  systems  that  retains  the  properties  of  the 
Kalman  filter  (i.e.  it  is  an  unbiased,  minimum  variance  estimator)  with  the  additional  ben¬ 
efits  of  additive  measurement  updates  and,  when  formulated  properly  as  discussed  below, 
additive  process  predictions  [26] . 

This  chapter  reports  a  brief  overview  of  the  extended  information  filter,  a  discussion  of 
sparsity  in  the  centralized  implementation  of  the  extended  information  filter  (CEIF),  and 
the  derivation  of  a  decentralized  extended  information  filter  (DEIF)  for  single -beacon  one- 
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way-travel-time  (OWTT)  navigation  [27,28,93,94].  The  CEIF  is  formulated  within  the 
the  same  framework  as  that  used  in  Chapter  5  for  the  CEKF,  i.e.  with  access  to  all  sensor 
data  from  both  the  vehicle  and  the  ship.  In  contrast,  the  DEIF  is  designed  to  run  locally 
on  a  submerged  vehicle  with  real-time  access  to  measurements  from  the  vehicle’s  inertial 
sensors  and  infrequent,  asynchronous  access  to  acoustic  broadcasts  from  a  moving  refer¬ 
ence  beacon.  The  DEIF  does  not  have  access  to  real-time  global  positioning  system  (GPS) 
measurements  from  the  reference  beacon  or  any  other  georeferenced  information  except 
that  which  is  received  acoustically.  While  the  CEIF  is  applicable  only  in  post-processing, 
the  DEIF  is  designed  to  be  implemented  in  real  time  on  one  or  more  vehicles.  Both  for¬ 
mulations  reported  here  are  for  the  nonlinear  plant  with  discrete  observations  described  in 
Chapter  4.  Note  that  the  CEIF  is  identical  to  the  CEKF  by  definition  because  they  are  for¬ 
mulated  with  identical  constraints.  The  CEIF  is  simply  based  on  a  different,  but  equivalent, 
pair  of  statistics  to  describe  the  state  of  the  system. 

The  remainder  of  this  chapter  is  organized  as  follows:  Section  6.2  reviews  the  extended 
information  filter.  Section  6.3  discusses  the  sparsity  of  the  centralized  information  filter  as  it 
is  formulated  for  OWTT  navigation.  Section  6.4  presents  the  derivation  of  the  decentralized 
extended  information  filter  and  shows  that  analytically  it  produces  identical  results  to  those 
of  a  centralized  extended  Kalman  filter  at  the  time  instant  immediately  following  each  range 
measurement.  Section  6.5  presents  the  results  of  a  numerical  simulation  used  to  validate 
the  DEIF.  Section  6.6  discusses  the  robustness  of  the  proposed  navigation  method  to  packet 
loss  and  strategies  for  mitigating  potential  issues  with  packet  loss.  Section  6.7  concludes. 
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6.2  Extended  Information  Filter 

The  information  filter  is  often  referred  to  as  the  dual  of  the  Kalman  filter.  The  Kalman 
filter  recursively  estimates  the  mean,  /i,  and  covariance,  X,  of  the  random  variable  x,  which 
is  the  state  vector  of  the  system 


/r  =  E[x\ 

(6.1) 

X  =  £  [(x-n)(x  -  /FT]  • 

(6.2) 

The  information  filter  is  based  on  the  explicit  normalization  of  the  random  variable  x  by 
its  covariance  X  such  that  the  information  filter  recursively  estimates  the  mean,  77,  and 
covariance,  A,  of  the  random  variable  X  1  x 


r]  =  E^x]  =  XV  (6.3) 

A  =  E  [(X_1x  -  X^/i^ir1®  -  X~V)T]  (6.4) 

=  E  [X^xaXX”1  -  X^x/^X”1  -  X-VaXX”1  +  X^^X”1]  (6.5) 

=  E  [X_1®a:TX_1]  -  X“V^TX_1  (6.6) 

=  X’1  (E  [xxT]  -  /i/aT)  X-1  (6.7) 

=  X'1  (6.8) 


where  77  is  referred  to  as  the  information  vector,  and  A  the  information  matrix  [8, 70]. 

The  information  filter  can  be  extended  for  use  on  nonlinear  systems  to  form  the  ex- 
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tended  information  filter  (EIF)  in  the  same  manner  as  the  extended  Kalman  filter — by 
linearizing  the  process  model  and  observation  models  along  the  trajectories  of  the  sys¬ 
tem  [36,70].  A  complete  derivation  of  the  linear  information  filter  is  provided  in  Appendix 
C.  The  process  prediction  and  measurement  update  equations  for  the  extended  information 
filter  are  well  known,  and  are  provided  here  with  references  but  without  proof. 

6.2.1  Conditioning  and  Marginalization 

Assuming  that  we  have  a  normal  random  variable  $,  such  that  p(£)  =  A/"(/x,  S),  parti¬ 
tioned  £  =  [ckt,  /3t]t  results  in 


(6.9) 


which  in  the  information  form  results  in  the  information  vector  and  information  matrix 


(6.10) 


(6.11) 


Examining  the  probabilistic  representation  of  the  information  filter  compared  to  the  Kalman 
filter,  we  see  that  the  operations  of  marginalization  and  conditioning  are  performed  by  the 
dual  calculations  shown  in  Table  6.1. 
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Table  6.1:  Kalman  filter  versus  information  filter 


Marginalization 

P(«)  =  I  p(<x,P)d(3 

Conditioning 

p(a\(3)  =p(a,P)/p(P) 

Kalman  filter 

X  =  XQQ 

^  =  Ma  +  ^‘ap'EppiP  ~  Vp) 

^  ^a/3^  /3f3^  (3  a 

Information  filter 

V  =  rla-  A«/?A ppflg 

A  ^aa  Aq^A^A.^, 

V'  =  Va  -  a<*pP 

A  Aaa 

Source:  Eustice  et  at.  [26] 


6.2.2  Process  Prediction 

We  assume  a  general  process  model  of  the  form 

x(t)  —  f(x(t),t)  +  G(t)w(t)  (6.12) 

where  x(t)  is  the  state  vector  and  w(t)  ~  J\f( 0,  Q(t))  is  independent  zero-mean  Gaussian 
process  noise  in  the  acceleration  term.  The  discrete-time  linearized  process  model  is  then 

xk+1  =  Fkxk  +  Bkuk  +  wk  (6.13) 

where  Fk  is  the  discrete-time  linear  state  transition  matrix,  Uk  is  the  piecewise-constant 
input,  and  wk  ~  A/"(0,  Qk)  is  independent  zero-mean  Gaussian  process  noise. 

In  the  formulation  of  the  information  filter  presented  herein  our  state  vector  consists  of 
both  current  and  historic  states  of  the  plant  defined  in  (6.13).  Therefore  we  consider  the 
state  vector  of  the  system,  denoted  by  a  different  font  x,  consisting  of  two  plant  states,  the 
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current  plant  state  xk  and  a  previous  plant  state  xp 


X-k\k 


Xk 

Xp 


with  the  information  matrix  and  vector  defined  as 


A-k\k 

r)k\k 


Afcfc 

A-pk 


A kp 

App 


Ik 


'Hp 


(6.14) 


(6.15) 

(6.16) 


In  the  information  form,  the  process  prediction  equations  for  the  system,  conditioned  on 
the  vehicle  sensor  measurements  up  to  time  k,  Z1:k,  and  the  control  inputs  up  to  time  k, 

U1:fc,  are 


p(**+1,*p|Z1:fc,U1:fc)  : 


Afc+i|fc 


Apfcfifc  F'kQk  A  pp  \pk^k  A-kp 
QklFk^klrlk  +  uk)  -  Fknk |fc) 

%  -  ApfcO"1^ 


(6.17) 

(6.18) 

(6.19) 
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where  /(•)  is  the  nonlinear  process  model  and 

(6.20) 
(6.21) 
(6.22) 


»*  =  («*  + **  Att1*?)-1 

=  A  kk  +  -Fj  <3^  lFk 
V*k  =  Vk  -  FlQk^ifi^k |*> 


as  derived  in  [26, 87]. 


6.2.3  Process  Prediction  with  Augmentation 

In  equations  (6.18)  and  (6.19),  the  current  state  at  time  k  is  propagated  to  time  k  +  1, 
such  that 


®fc+i 

Xp 


(6.23) 


If,  instead,  we  augment  the  state  vector  to  include  the  state  at  time  k  +  1  in  addition  to  the 
original  states 


•®fc+i 

Xp 


(6.24) 
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the  process  prediction  equations  have  a  very  different  structure  [26], 


l:k  TTl:fe\ 


p(xk+1,xk,xp\Zhlc,U 


A 


k-\-l\k 


Vk+l^ 


Qk 


K1 

-FjQ*1 


-Qk'Fk  0 

Fk  Qk  F k  T  A.kk  h-kp 

0  A-pk  A.pp 

QkHfiPqk’Uk)  -FkfJ.k\k) 

Vk  ~  FlQk\f{vk \k,  uk)  -  Fknk  ]k 
% 


(6.25) 

(6.26) 


(6.27) 


where  Equation  (6.26)  can  be  written  as  the  sum  of  two  matrices — one  containing  the 
process  prediction  information  and  the  other  containing  the  previous  information  matrix 


Qk1 

-Qi'Ft  0  ' 

"  0 

0 

0 

~ FlQ -1 

FlQ-tlFt  0 

+ 

0 

A-kk 

A-kp 

0 

0  0 

0 

A-pk 

A-pp 

(6.28) 


As  is  noted  in  [26],  this  results  in  A  having  a  sparse,  block-tridiagonal  structure.  The 
sparsity  of  A  is  examined  in  greater  detail  in  Section  6.3. 


6.2.4  Measurement  Update 

Assuming  that  we  have  the  discrete,  nonlinear  observation  model 


zk  =  h(x(tk ))  +  vk,  k  =  1, 2,  — 


(6.29) 
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where  zk  is  the  measurement  at  time  step  tk,  xk  is  the  state  vector,  h(-)  is  the  observation 
model,  and  vk  ~  A/"(0,  Rk)  is  independent  zero-mean  Gaussian  measurement  noise,  the 
measurement  update  equations  for  the  extended  information  filter  are 


—  Afc|fc_!  +  HjR^H,  (6.30) 

=rlk\k-i  +  HkRk  1  (zk  —  h(nk  |fe_r)  +  Hf :Hk\k-i)  (6.31) 


where  Hk  is  the  Jacobian  of  the  measurement  with  respect  to  x 


// 


dh(x(tk)) 

dx(tk) 


(6.32) 


and  nk\k_i  is  the  mean  of  the  state  vector  [36,70,87].  Note  that  one  of  the  useful  properties 
of  the  information  filter,  shown  clearly  in  (6.30)  and  (6.31),  is  that  the  measurement  update 
equation  is  additive. 


6.3  Sparsity  in  the  Centralized  Filter 

The  purpose  of  using  the  information  filter  is  to  enable  a  tractable  decentralized  im¬ 
plementation  for  one-way-travel-time  navigation.  Before  deriving  the  decentralized  infor¬ 
mation  filter,  however,  we  briefly  explore  sparsity  in  the  centralized  extended  information 
filter  (CEIF)  within  the  context  of  OWTT  navigation. 

The  CEIF  uses  the  identical  model  described  in  chapter  4.  As  described  in  [26],  the 
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information  matrix  can  be  represented  graphically  using  undirected  graphical  models.  Fig¬ 
ure  6.1  depicts  two  such  models  of  the  vehicle  and  the  ship,  where  the  nodes,  xVi  and  xSi 
for  i  e  [!,•••  ,  n],  represent  the  pose  of  the  vehicle  or  ship  at  a  different  time  step  and 
the  edges  represent  constraints  between  poses.  Note  that,  though  the  graph  is  undirected, 
arrows  are  used  for  the  convenience  of  the  readers  to  represent  the  temporal  dependence  of 
the  poses.  All  of  the  poses  shown  are  included  in  the  state  vector  of  the  system 


x  =  \x 


vo ’ 


xT  xT 


,<]T- 


(6.33) 


In  the  information  form,  constraints  also  represent  non-zero  entries  in  the  corresponding 
information  matrix.  In  the  information  matrices  shown,  the  vehicle  poses  are  grouped 
together  in  the  upper  left  sub-block  and  the  ship  poses  are  grouped  together  in  the  bottom 
right  sub-block  as  shown  in  (6.33).  The  current  ship  and  vehicle  states,  xV6  and  xS6,  are  in 
the  top  left  comer  of  their  respective  sub-blocks. 

As  is  noted  in  [26]  and  shown  in  Section  6.2.3,  when  the  information  filter  is  formulated 
such  that  the  state  vector  contains  an  estimate  of  multiple  poses,  the  resulting  information 
matrix,  A,  has  a  sparse,  tridiagonal  structure.  This  tridiagonal  structure  is  clearly  visible 
from  the  graphical  model  because  each  pose  is  only  connected  to  the  pose  immediately 
before  it  in  time  and  the  pose  immediately  after  it  in  time.  Mathematically  this  represents 
the  conditional  independence  of  the  current  pose,  given  the  previous  pose,  of  all  other 
historic  poses — a  property  of  first-order  Markov  processes  [78]. 
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(a)  Undirected  graphical  models  for  the  vehicle  and  ship,  where  xVji  and  xSn  are  the  vehicle  and 
ship  states  respectively  at  time  step  n.  The  associated  information  matrix  is  sparse  and  tridiagonal. 


(b)  Range  measurements  between  the  ship  and  vehicle  add  additional  constraints  shown  in  blue. 

Figure  6.1:  Example  undirected  graphical  models  and  their  corresponding  information  ma¬ 
trices. 

In  Figure  6.1,  the  graphical  model  in  (a)  shows  the  vehicle  and  ship  independent  of  each 
other.  The  addition  of  range  measurements  in  (b)  adds  constraints  between  the  vehicle  and 
ship  and  adds  non-zero  off-diagonal  entries  to  the  information  matrix.  Because  every  ship 
or  vehicle  pose  has  at  most  one  range  measurement  associated  with  it  (in  the  single-vehicle 
implementation),  the  range  measurements  add  only  a  single  diagonal  above  and  below 
the  tridiagonal.  Measurements  from  other  vehicle  or  ship  sensors  do  not  add  additional 
constraints  because  they  only  add  information  to  the  current  vehicle  or  ship  state. 

The  marginalization  of  states  in  the  system  has  the  potential  to  reduce  sparsity  by  caus¬ 
ing  fill-in.  Graphically,  when  a  state  is  marginalized  out,  every  state  linked  to  it  in  the  pose 
graph  becomes  simply  connected  as  shown  in  Figure  6.2.  When  marginalizing  out  a  state 
that  does  not  have  a  range  measurement  associated  with  it,  as  in  (a),  the  tridiagonal  struc¬ 
ture  of  the  information  matrix  is  not  altered.  However,  when  the  ship  and  vehicle  states 
associated  with  one  of  the  range  measurement  are  marginalized  out,  as  in  (b),  the  resulting 
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graphical  has  a  densely  connected  subset  of  states.  In  the  CEIF  we  avoid  this  problem  by 
marginalizing  out  only  the  oldest  vehicle  and  ship  states  as  shown  in  (c).  In  this  case,  when 
marginalizing  out  both  the  ship  and  vehicle  states  associated  with  the  oldest  time  step  that 
has  a  range  measurement  associated  with  it,  the  constraint  from  the  original  range  mea¬ 
surement  is  replaced  by  a  link  between  the  oldest  remaining  vehicle  and  ship  states,  but  no 
additional  links  are  added  to  the  graphical  model. 


(a)  The  marginalization  of  a  state  that  is  only  serially  connected  with  the  states  before  and  after  it 
has  no  effect  on  the  sparsity  or  tridiagonal  structure  of  the  information  matrix. 


(b)  During  the  marginalization  of  the  states  associated  with  a  range  measurement,  fill-in  occurs 
in  the  information  matrix. 


(c)  The  marginalization  of  only  the  oldest  states  prevents  fill-in  in  the  information  matrix,  even  in 
the  presence  of  range  measurements. 

Figure  6.2:  Undirected  graphical  models  illustrating  the  effects  of  marginalization.  Vehicle 
and  ship  states  are  shown  for  every  tTOL  and  tTOA. 
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6.4  Decentralized  Implementation 

In  this  section  we  present  the  derivation  of  the  vehicle-based  decentralized  extended 
information  filter  (DEIF),  designed  to  enable  the  simultaneous  navigation  of  multiple  un¬ 
derwater  vehicles  in  real  time  using  only  vehicle-based  navigation  sensors  and  acoustic 
broadcasts  from  the  ship  or  other  georeferenced  node.  The  implementation  of  the  DEIF 
relies  on  two  separate  filters,  a  ship-based  filter  and  a  vehicle-based  filter,  both  of  which 
process  sensor  data  causally  and  asynchronously.  The  information  filter  on  the  ship  has 
access  to  ship  sensor  data  but  not  range  measurements.  The  ship-based  filter  is  used  to  cal¬ 
culate  the  change,  or  delta,  in  the  ship  infoimation  vector  and  information  matrix  between 
acoustic  broadcasts,  and  this  delta  information  is  acoustically  transmitted  to  the  vehicle. 
Figure  6.3  shows  a  schematic  of  the  delta  ship  information  transmitted  from  the  ship  to  the 
vehicle.  The  decentralized  vehicle-based  filter,  the  DEIF,  has  real-time  access  to  vehicle 
sensor  data  and  the  asynchronous  acoustic  broadcasts  from  the  ship,  but  does  not  have  di¬ 
rect  access  to  the  ship  sensor  measurements  apart  from  the  delta  information  transmissions. 


6.4.1  DEIF  State  Vector 

The  DEIF,  the  filter  on  the  vehicle,  maintains  an  estimate  of  the  current  vehicle  state  as 
well  as  estimates  of  historic  ship  states  reconstructed  from  the  delta  information  received 
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Figure  6.3:  A  schematic  of  the  information  contained  in  the  acoustic  data  packet  transmitted 
from  the  ship  to  the  vehicle. 


acoustically  from  the  ship 


X-k\k 


Xyk 

XSTOLn 


(6.34) 


XsTOl1 

where  x*.i k  denotes  the  entire  vehicle  state  vector,  xVk  denotes  the  current  vehicle  state,  and 
xsToLn  denotes  the  ship  state  at  the  time-of-launch  (TOL)  of  the  nth  acoustic  data  packet. 
In  practice  it  is  undesirable  and  unnecessary  to  keep  every  ship  state,  causing  the  state 
vector  to  grow  without  bound.  Instead  we  maintain  a  fixed-length  queue  of  ship  states  by 
marginalizing  out  the  oldest  ship  state  when  a  new  state  is  appended  to  the  state  vector. 
Note  that,  because  acoustic  broadcasts  have  a  non-negligible  time  delay  associated  with 
them,  the  DEIF  state  vector  cannot  contain  information  about  the  current  ship  state,  it  only 
contains  information  about  historic  ship  states  (see  Figure  6.3). 

The  reconstructed  ship  states  are  not  subjected  to  process  predictions  or  measurement 
updates  other  than  range  measurements,  because  the  DEIF  has  no  knowledge  of  the  ship 
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process  model  or  measurement  from  ship  sensors.  The  ship  states  remain  unchanged  over 
time  except  when  new  information  from  acoustic  broadcasts  is  incorporated  and  range 
measurement  updates  are  performed. 


6.4.2  Independent  Ship  Filter 

The  ship-based  filter  maintains  an  estimate  of  the  current  ship  state  as  well  as  estimates 
of  previous  ship  states  at  the  time-of-launch  (TOL)  of  each  acoustic  data  packet.  Assuming 
n  data  packets  have  been  broadcast,  this  results  in  a  state  vector  of  the  form 


x 


X 


STOLn 


(6.35) 


•*sTOi2 

.  '^STOL1  . 


where  xSjfc|fe  denotes  the  entire  state  vector  at  time  k,  xSk  denotes  the  current  ship  state,  and 
xsToLn  denotes  the  ship  state  when  the  nth  acoustic  data  packet  was  broadcast.  In  practice 
it  is  undesirable  and  unnecessary  to  keep  every  ship  state,  causing  the  state  vector  to  grow 
without  bound.  Instead  we  maintain  a  fixed-length  queue  of  ship  states  by  marginalizing 
out  the  oldest  ship  state  when  a  new  state  is  appended  to  the  state  vector. 


92 


CHAPTER  6.  EXTENDED  INFORMATION  FILTER 


6.4.3  Acoustic  Range  Measurements 

To  initiate  a  range  measurement,  the  ship  broadcasts  an  acoustic  data  packet  containing 
information  about  the  ship  state.  When  a  data  packet  packet  arrives  at  the  vehicle,  the  ship 
information  included  in  the  data  packet  is  incorporated  into  the  DEIF  in  order  to  recreate 
the  ship’s  filter.  After  the  ship  information  is  incorporated,  the  range  measurement  update, 
(6.30)  and  (6.31),  is  performed  using  the  range  information  garnered  from  the  acoustic 
broadcast. 

What  differentiates  the  DEIF  from  other  estimators  used  in  decentralized  single -beacon 
navigation  is  the  information  that  is  transmitted  with  the  range  measurements  and  how  that 
information  is  incorporated  into  the  decentralized  vehicle  navigation  filter  in  conjunction 
with  the  range  measurement.  In  other  formulations  of  single -beacon  navigation,  the  acous¬ 
tic  data  packet  contains  the  mean  and  covariance  of  the  ship’s  current  x-y  position,  which 
is  used  by  the  filter  on  the  vehicle  to  perform  a  range  measurement  update.  In  [5]  and  [29] 
in  particular,  which  are  the  works  that  most  closely  resemble  the  work  presented  in  this 
thesis,  range  measurements  are  incorporated  in  an  ad  hoc  fashion  where  multiple  hypothe¬ 
ses  for  current  and  historic  vehicle  states  are  found  based  on  the  intersection  of  the  current 
range  measurement  and  a  given  number  of  historic  range  measurements  are  forward  propa¬ 
gated  using  the  vehicle’s  dead-reckoned  track.  The  likelihood  of  the  vehicle’s  path  through 
each  combination  of  these  possible  states  is  evaluated  with  a  cost  function  based  on  the 
Kullback-Leibler  divergence  distance  metric.  Over-confidence  is  addressed  by  maintain¬ 
ing  multiple  filters  on  each  vehicle  that  selectively  exclude  data  from  every  combination 
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of  other  vehicles.  The  acoustic  broadcast  from  a  vehicle  contains  the  estimates  from  all  of 
these  filters  so  that  the  receiving  vehicle  is  able  to  choose  an  estimate  that  does  not  result  in 
an  over-confident  solution.  As  a  result,  the  proposed  algorithm  is  somewhat  cumbersome 
and  difficult  or  impossible  to  compare  analytically  to  a  centralized  filter. 

In  contrast,  in  the  formulation  presented  here,  the  acoustic  data  packet  contains  the 
change  in  As  and  rjs  between  the  time  of  the  current  acoustic  broadcast,  TOLn,  and  the 
time  of  the  previous  acoustic  broadcast,  TOLn_i, 


/\ATOLn  KSTOLn  AStol^^ 

^WrOLn  —  VsTOLn  ~  ^ STOLn_  1 


(6.36) 

(6.37) 


where,  for  conformability,  AStol  _i  and  r/Sro/  |  have  been  padded  with  zeros  to  match 
the  size  of  ASTOLn  and  f)STOL  respectively.  These  data  packets  are  reassembled  subsea  in 
the  DEIF  to  recreate  the  ship’s  filter  and,  as  discussed  in  more  detail  in  Section  6.4.4,  enable 
the  vehicle-based  filter  to  exactly  replicate  the  results  of  the  centralized  filter  immediately 
after  each  range  measurement.  The  structure  of  the  ship-based  filter  is  the  key  to  making 
this  replication  possible. 

Because  of  the  linear  ship  model  and  the  linear  observation  models  for  GPS  and  gyro¬ 
compass  measurements,  no  linearization  is  necessary  to  calculate  the  process  prediction  or 
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the  measurement  update.  The  process  prediction 


A 


(6.38) 


(6.39) 


is  additive  and  independent  of  the  ship  state — FSk  is  constant  and  Q  is  dependent  only 
on  the  size  of  the  time  step.  The  measurement  update 


(6.40) 


Hk+iRk+lHk+i  0 


fc+iJXfc+i 

0 


(6.41) 


0 


(6.42) 


is  also  additive  and  depends  only  on  the  observation  models  and  the  measurement  noise. 
In  the  case  of  the  ship’s  GPS  and  gyrocompass  observation  models,  both  are  linear  because 
they  are  directly  measuring  elements  in  the  state  vector  (x-y  position  and  heading)  and  we 
assume  a  constant  measurement  noise  covariance  R  for  each  that  does  not  vary  with  time. 
The  delta  information  after  a  process  prediction  and  measurement  update 


(6.43) 


(6.44) 
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is  thus  completely  independent  of  the  ship  state  because  every  element  in  the  matrix  is 
constant.  In  addition,  a  single  delta  measurement  encapsulates  all  of  the  ship  sensor  infor¬ 
mation  gained  since  the  last  acoustic  data  packet  was  transmitted. 

At  the  vehicle,  when  the  delta  information  is  received,  it  is  incorporated  into  the  DEIF 
by  addition  after  accounting  for  conformability.  We  assume  that  the  range  measurement 
was  broadcast  at  the  ship  at  t  —  k  +  1  and  arrives  at  the  vehicle  at  t  =  m  +  1  where  m  >  k. 
Let  A“+1|m  represent  the  DEIF  information  matrix  before  the  addition  of  the  delta  ship 
information  and  A+  + ,  ,  represent  the  DEIF  information  matrix  after  the  addition  of  the 
delta  ship  information.  We  assume  that 


A 


m+l\m 


A  A 

vm+\\m  rngm 
A-rngm  A  Sfc|fc 


(6.45) 


before  the  delta  ship  information  is  incorporated,  where  Arngm  is  the  term  from  the  previous 
range  measurement  at  t  —  m  that  correlates  the  current  vehicle  state  and  the  ship  state  at 
t  =  k.  After  the  delta  ship  information  is  incorporated 
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Incorporating  the  new  range  measurement  that  is  made  at  t  —  m  +  1  when  the  delta  ship 
information  arrives 


Am+l|m+l  —  Am+l|m  +  H  rngm+i  RrngH rngm+1 


(6.49) 


where 
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correlates  the  current  vehicle  state  and  the  ship  state  at  k  +  1  such  that 
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Because  the  elements  that  comprise  the  delta  information  are  constant,  i.e.  independent 
of  the  actual  ship  state,  the  independent  ship  filter  and  the  centralized  filter  will  calculate 
identical  information  from  process  predictions  and  measurement  updates.  Thus  AATOLk+1 
and  ArjTOLk+i  calculated  from  the  ship-based  filter,  which  has  no  knowledge  of  the  vehicle 
and  is  not  subject  to  range  measurement  updates,  is  identical  to  the  CEIF  even  though 
the  two  filters  have  different  estimates  of  the  ship’s  current  state.  The  simplicity  of  this 
computation  is  one  of  the  advantages  of  the  information  filter. 

6.4.4  Comparison  between  DEIF  and  CEKF 

The  centralized  extended  Kalman  filter  (CEKF)  that  has  simultaneous,  real-time  access 
to  both  vehicle  and  ship  sensor  data  is  used  as  a  benchmark  for  state  estimation  perfor¬ 
mance.  The  goal  of  the  DEIF  is  for  the  current  state  of  the  vehicle  recovered  from  the  DEIF 
to  exactly  reproduce  the  vehicle  state  recovered  from  the  CEKF.  The  DEIF  accomplishes 
this  immediately  after  each  range  measurement  update,  but  there  are  several  subtleties  to 
this  operation  that  we  address  here. 

The  range  measurement  is  a  nonlinear  measurement.  In  order  for  the  range  measure¬ 
ment  update  made  by  the  DEIF  to  match  that  made  by  the  CEKF,  both  filters  must  be 
linearizing  the  range  observation  model  about  the  same  vehicle  state.  At  the  TOA  of  the 
acoustic  data  packet,  the  range  measurement  is  made  between  xVTOA  and  xSTOL .  Compar¬ 
ing  the  probability  distributions  of  the  two  filters  immediately  after  the  range  measurement 
update,  we  find  that  they  are  not  identical  because,  as  shown  in  Figure  6.4,  the  centralized 
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filter  has  access  to  the  ship-based  measurements  shown  in  blue,  while  the  decentralized 
filter  does  not, 
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(^xvk,  xsTOl 
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where  is  the  most  recent  range  measurement,  Z  ’;TO/'  are  the  vehicle  sensor  measure¬ 
ments  up  to  the  TOA,  Uj,:TOA  are  the  vehicle  control  inputs  up  to  the  TOA,  and  Z ls-TOL  are 
the  ship  sensor  measurements  up  to  the  TOL.  The  ramifications  of  this  are  that  the  DEIF 


Figure  6.4:  The  centralized  filter  has  access  to  ship  sensor  measurements,  shown  in  blue, 
that  are  unavailable  to  the  decentralized  filter  when  the  range  measurement  is  received. 

performs  a  range  measurement  between  the  current  vehicle  state  and  the  best  estimate  of 
the  ship  state  at  the  TOL  given  ship  sensor  measurements  only  up  to  the  TOL.  In  contrast, 
the  CEKF  performs  a  range  measurement  between  the  current  vehicle  state  and  the  best 
estimate  of  the  ship  state  at  the  TOL  given  ship  sensor  measurement  up  to  the  TOA.  The 
CEKF  is  essentially  performing  a  smoothing  operation  on  the  ship  state  at  the  TOL,  because 
it  has  additional  information  from  the  ship  sensors  after  the  data  packet  was  broadcast. 
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In  order  to  properly  compare  the  CEKF  and  the  DEIF,  we  use  a  two-step  delayed  mea¬ 
surement  update  in  the  CEKF,  first  performing  a  measurement  update  for  the  range  mea¬ 
surement  that  only  includes  ship  measurements  up  until  the  TOL,  (6.59),  and  then  per¬ 
forming  another  measurement  update  for  the  ship  measurements  that  happened  between 
the  TOL  and  the  TOA,  (6.60) 
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CEKF  (step  2):  p(xVh,  xSTOL 
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Now  the  DEIF  distribution  in  (6.57)  is  identical  to  the  CEKF  distribution  in  (6.59),  without 
compromising  the  CEKF’s  final  distribution,  i.e.  (6.60)  is  identical  to  (6.58). 

Between  range  measurements,  the  CEKF  and  DEIF  estimates  of  the  vehicle  state  will 
not  be  identical  because  of  linearization  errors,  as  seen  in  (6.57)  versus  (6.58).  However, 
at  the  instant  of  every  range  measurement,  through  the  two-step  delayed  update,  the  filter 
estimates  will  be  made  identical  again. 


6.5  Simulation 

The  performance  of  the  DEIF  in  comparison  to  the  CEKF  is  demonstrated  using  a 
simulated  6-hour  survey  at  3800m  depth.  The  CEKF  has  access  to  the  sensor  measurements 
from  both  the  ship  and  the  vehicle  simultaneously.  The  DEIF  has  real-time  access  to  vehicle 
sensor  data  and  asynchronous  acoustic  broadcasts  from  the  ship  that  are  used  to  make  range 
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measurements.  The  DEIF  does  not  have  access  to  the  ship  sensor  measurements  apart  from 
these  acoustic  broadcasts.  To  test  the  validity  of  the  DEIF,  we  compare  the  state  estimate 
recovered  from  it  to  that  obtained  from  the  CEKF  using  the  two-step  delayed  measurement 
update  as  described  in  Section  6.4.3. 

6.5.1  Simulation  Setup 

This  simulation  is  designed  to  mimic  the  experimental  setup  of  the  deep  water  survey 
described  in  Section  5.3  and  [93].  In  the  simulated  mission  presented  here,  the  vehicle 
drives  ten  700  m  tracklines  spaced  80  m  apart  at  a  velocity  of  0.35  m/s.  The  vehicle  depth 
is  constant  at  3800  m.  The  vehicle  takes  approximately  6  hours  to  complete  the  survey, 
during  which  time  the  ship  drives  around  the  vehicle  survey  area  in  a  diamond  pattern  at 
0.5  m/s,  broadcasting  acoustic  data  packets  every  2.5  minutes. 

We  assume  that  the  ship  is  equipped  with  a  differential  global  positioning  system  (DGPS) 
receiver  and  a  gyrocompass  to  measure  heading.  The  vehicle  has  an  OCTANS  fiber-optic 
gyrocompass  to  measure  attitude  and  attitude  rates;  a  Paroscientific  pressure  sensor  to  mea¬ 
sure  depth;  and  an  RDI  Doppler  velocity  log  (DVL)  to  measure  bottom-referenced  veloc¬ 
ities.  Acoustic  modems  are  used  to  measure  the  range  between  the  ship  and  the  vehicle. 
The  vehicle  and  ship  navigation  sensors,  their  sampling  frequencies,  and  the  noise  statistics 
for  each  sensor  are  given  in  Table  6.2,  where  ip,  6,  and  o  are  local-level  heading,  pitch,  and 
roll  respectively;  r,  q,  and  p  are  body-frame  angular  rates  in  heading,  pitch,  and  roll. 

In  order  to  compare  the  CEKF  and  the  DEIF,  we  must  initialize  them  to  the  same  point. 
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Table  6.2:  Simulated  navigation  sensor  sampling  frequency  and  noise. 


Sensor 

Frequency 

Noise 

OCTANS 

3  Hz 

V>:  0.1° 

<l>,6:  0.01° 
r:  0.5°/s 
p,  q:  0.25°/s 

depth  sensor 

0.9  Hz 

5  cm 

DVL 

3.0  Hz 

1  cm/s 

GPS 

1.0  Hz 

0.5  m 

gyrocompass 

2.0  Hz 

0.1° 

modem 

every  2.5  min 

4  m 

In  this  simulation,  for  comparison  purposes  we  initialize  the  vehicle  state  to  the  true  vehicle 
position  with  a  large  covariance  for  both  filters.  During  an  actual  mission  this  would,  of 
course,  not  be  possible.  Instead  the  vehicle  would  estimate  its  initial  own  position  using  a 
method  such  as  a  maximum  likelihood  estimate  over  the  first  few  range  measurements. 


6.5.2  DEIF  Results 

A  comparison  between  the  DEIF  and  the  CEKF  for  the  simulated  dive  is  shown  in 
Figures  6.6,  6.7,  and  6.5.  Figure  6.5  shows  the  estimated  vehicle  trajectory  overlaid  with 
the  3-sigma  covariance  of  the  vehicle  position  as  estimated  by  the  DEIF.  The  GPS-reported 
position  of  the  ship  as  it  moves  around  the  vehicle  survey  area  is  also  shown.  Figure  6.6 
shows  the  difference  between  the  true  vehicle  position  and  the  estimate  from  the  DEIF 
of  the  vehicle  position  over  the  course  of  the  simulated  dive.  The  3-sigma  error  bars  are 
included  to  show  that  the  filter  maintains  consistency  over  the  course  of  the  dive.  The 
error  at  the  end  of  the  dive  between  the  DEIF’s  estimate  of  the  vehicle  position  and  the 
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Combined  Vehicle  and  Ship  Trajectory 


Figure  6.5:  Ship  and  vehicle  trajectories.  The  ship  moved  counter-clockwise  around  the 
diamond  starting  at  the  eastern-most  apex. 

true  vehicle  position  is  3.7  m  cross-track  and  0.2  m  along-track  both  with  3.1  m  standard 
deviation.  For  comparison,  had  the  vehicle  relied  solely  on  dead  reckoning  throughout  the 
dive  with  no  range  measurements,  the  error  at  the  end  of  the  dive  between  the  estimated  and 
true  vehicle  position  would  have  been  8.8  m  cross-track  and  5.6  m  along-track  with  a  7.8 
m  standard  deviation.  Comparing  the  estimated  mean  of  the  12  DOF  vehicle  state  vector 
from  the  DEIF  versus  the  CEKF,  Figure  6.7  shows  the  norm  of  the  difference  of  the  state 
vector,  (AxT  Ax)-1/2  where  Ax  =  xDEif  —  X-cekf,  over  the  course  of  the  simulation.  The 
lower  plot  highlights  the  norm  of  the  difference  immediately  after  the  range  measurements 
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A  X  and  Y  position:  DEIF  estimated  minus  true  with  3-cr  error  bars 


Figure  6.6:  The  difference  between  the  true  vehicle  position  and  estimate  from  the  DEIF 
over  time.  The  error  bars  are  3  times  the  standard  deviation  in  each  degree  of  freedom. 

marked  by  asterisks.  Note  that  the  y-axis  on  the  lower  plot  has  been  scaled  down  by  two 
orders  of  magnitude  to  show  the  precision  with  which  the  DEIF  is  able  to  reproduce  the 
results  of  the  CEKF.  As  expected,  the  x-y  position  error  dominates  the  norm.  The  average 
difference  between  the  filters  across  the  entire  dive  is  5.68e-3  m  (5.7  mm)  in  x-y  position 
and  3.35e-8  in  the  other  state  elements.  The  average  difference  immediately  after  a  range 
measurement  is  8.27e-5  m  in  x-y  position  and  1.70e-10  in  the  other  vehicle  states. 
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Norm  of  the  Difference  in  Vehicle  State 
between  DEIF  and  CEKF 


x  10 


Figure  6.7:  (a)  The  sum  of  the  squared  error  between  the  mean  vehicle  position  as  estimated 
by  the  DEIF  versus  the  CEKF.  (b)  The  same  plot  as  (a)  but  with  a  zoomed  view  of  the  y- 
axis  highlighting  the  norm  immediately  after  the  range  measurements. 


6.5.3  Discussion 


As  discussed  in  Section  6.4.4,  we  expect  the  DEIF  to  produce  state  estimates  that  are 
comparable  to  the  CEKF:  immediately  after  each  range  update  the  results  should  be  iden¬ 
tical;  between  range  updates,  the  results  should  differ  only  due  to  linearization  errors.  The 
results  from  the  simulation  shown  in  Figure  6.7  support  this  within  the  tolerance  of  numer¬ 
ical  precision.  Over  the  course  of  the  6-hour  simulated  dive  the  difference  in  x-y  position 
between  the  DEIF  and  the  CEKF  is,  on  average,  8.27e-5  m  immediately  after  each  range 
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update.  In  addition,  the  difference  between  the  filters  due  to  linearization  errors  (averaged 
over  the  entire  dive)  is  5.7  mm  on  average  with  a  maximum  difference  of  4.9  cm. 

Because  these  results  are  based  on  a  simulated  data  set,  there  are  several  possible  dis¬ 
crepancies  compared  to  experimental  data.  The  assumed  noise  characteristics  of  the  navi¬ 
gation  sensors  in  Table  6.2  are  used  both  in  the  simulation  of  noisy  sensor  data  and  in  the 
observation  models  in  the  DEIF  and  CEKF.  As  a  result  the  observation  models  exactly 
predict  the  performance  of  the  navigation  sensors.  In  addition,  the  noise  for  every  sensor  is 
assumed  to  be  Gaussian.  While  these  assumptions  may  be  reasonable  for  common  vehicle 
navigation  sensors  that  have  been  tested  extensively  in  the  field  [54],  acoustic  range  mea¬ 
surements  suffer  from  highly-variable,  non-Gaussian  noise  sources  including  multi-path 
and  ray-bending  errors.  In  an  attempt  to  account  for  this,  we  use  a  large  assumed  vari¬ 
ance  for  the  range  measurements.  In  a  real-world  context,  outlier  filtering  of  the  range  data 
would  be  necessary. 

6.6  Robustness  to  Acoustic  Data  Packet  Loss 

Packet  loss  occurs  when  acoustically  transmitted  data  packets  are  not  successfully  re¬ 
ceived  and  decoded  by  the  vehicle.  Packet  loss  can  occur  for  a  variety  of  reasons,  including 
poor  signal-to-noise  ratio,  attenuation  due  to  range,  or  acoustic  collision  because  of  an 
acoustic  multi-path  or  other  acoustic  transmissions.  Packet  loss  is  a  legitimate  operational 
concern  given  the  structure  of  the  algorithm  presented  here,  whereby  incremental  informa- 
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tion  about  the  ship’s  filter  is  acoustically  broadcast  by  the  ship  and  reconstructed  by  the 
vehicle. 

Operationally  there  exist  several  possible  implementations  that  address  packet  loss. 
Contingent  on  the  available  bandwidth,  the  AA  for  the  last  k  broadcasts  can  be  included 
in  each  acoustic  broadcast.  In  addition,  in  the  field  vehicle  operations  are  typically  moni¬ 
tored  via  acoustic  broadcasts  from  the  vehicle  that  contain  basic  vehicle  health  and  status 
updates.  To  mitigate  the  possibility  of  packet  loss,  the  vehicle  status  updates  will  include 
the  time  of  the  last  range  measurement  successfully  received  at  the  vehicle,  allowing  the 
surface  beacon  to  adjust  its  delta  measurement  broadcasts  accordingly.  We  are  also  con¬ 
tinuing  to  investigate  alternate  structuring  of  the  delta  information  that  is  broadcast  from 
the  ship  in  order  to  enable  the  DEIF  to  exactly  recreate  the  delta  information  regardless  of 
packet  loss.  Robustness  to  packet  loss  and  fully  exploring  the  ramifications  of  packet  loss 
on  the  DEIF’s  estimate  of  vehicle  position  over  time  are  both  areas  for  further  research. 

6.7  Chapter  Summary 

The  structure  of  the  information  filter  makes  it  a  natural  choice  for  a  decentralized  im¬ 
plementation  where  the  available  bandwidth  for  sending  data  between  nodes  is  limited. 
Delayed  ship  updates  are  simply  additive  and  require  a  minimal  amount  of  information  to 
be  acoustically  transmitted  that  is  well  within  the  functional  limits  of  available  acoustic 
modems  [30,31].  In  this  chapter  we  have  derived  a  vehicle-based  extended  information  HI- 
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ter  that  is  able  to  estimate  a  vehicle’s  state,  including  x-y  position,  using  only  vehicle-based 
inertial  sensors  and  asynchronous  acoustic  broadcasts  from  a  single,  moving,  georeferenced 
beacon.  The  DEIF  is  able  to  locally  recreate  vehicle  state  estimates  that  are  commensurate 
with  the  results  from  a  centralized  extended  Kalman  filter  within  a  margin  of  numerical 
error,  and  the  filter  did  so  over  the  course  of  a  simulation  that  is  representative  of  an  actual, 
deep-water  survey  in  both  physical  scale  and  the  frequency  of  measurements.  In  addition, 
the  filter  in  its  current  form  can  be  utilized  on  multiple  underwater  vehicles  where  each 
vehicle  simultaneously  receives  acoustic  data  broadcasts  from  the  reference  beacon.  Given 
the  favorable  results  in  simulation  of  the  DEIF,  we  look  forward  to  experimentally  validat¬ 
ing  this  algorithm  and  continuing  to  work  towards  a  full  multi-vehicle  implementation. 
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Conclusion 


The  chirp  of  the  underwater  modem  heralds  a  new  age  of  communication,  navigation, 
and  control  for  underwater  vehicles.  The  ability  of  a  shipboard  science  and  operations  team 
to  communicate  with  an  autonomous  vehicle  that  is  underway  thousands  of  meters  deep 
has  the  potential  to  revolutionize  autonomous  vehicle  operations.  Receiving  science  data 
in  near  real  time  or  sending  commands  and  new  mission  plans  to  the  vehicle  acoustically 
without  risking  the  hazards  or  wasting  the  time  of  recovery  and  launch  provides  improve¬ 
ments  in  the  duty-cycle  of  vehicle  operations,  flexibility  in  mission  programming,  and  the 
ability  to  monitor  in  detail  the  progress  of  the  vehicle  both  operationally  and  scientifically. 
The  emerging  field  of  acoustic  underwater  communications  also  has  the  potential  to  en¬ 
able  collective  or  collaborative  multi-vehicle  behavior  that  has  thus  far  been  the  purview  of 
land,  air,  and  space  vehicles  because  of  their  access  to  wireless  networks  for  inter-vehicle 
communication.  The  contributions  of  this  thesis  are  based  on  combining  acoustic  commu- 
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nication  with  navigation  in  order  to  eliminate  the  need  for  costly  external  acoustic  beacons 
while  providing  a  distributed,  scalable  precision  navigation  system. 

7.1  Summary 

This  thesis  presents  the  hardware,  software,  and  algorithms  designed  to  perform  de¬ 
centralized  single -beacon  one- way-travel-time  navigation  for  underwater  vehicles.  The 
Acomms  system,  consisting  of  the  hardware  and  software  necessary  for  single-beacon  one- 
way-travel-time  navigation,  has  proven  its  effectiveness  for  managing  acoustic  communi¬ 
cation  on  four  oceanographic  expeditions  to  date,  including  multiple  dives  to  more  than 
10,900  m  depth  in  the  Challenger  Deep  in  the  Mariana  Trench  with  the  HROV  Nereus. 

We  verified  the  ability  of  the  centralized  single-beacon  navigation  algorithm  to  pro¬ 
vide  vehicle  state  estimates  that  have  an  accuracy  commensurate  with  that  of  long  baseline 
acoustic  navigation  in  deep  water  using  navigation  data  collected  by  the  author  and  col¬ 
laborators  with  the  Acomms  system  during  an  AUV  survey  in  4000  m  of  water.  The  state 
estimate  of  the  decentralized  vehicle-based  single-beacon  navigation  algorithm  was  shown 
to  be  identical  to  the  state  estimate  from  the  centralized  filter  at  the  instant  of  each  range 
measurement,  and  to  differ  by  only  linearization  errors  between  range  measurements.  This 
conclusion  is  supported  by  the  results  from  a  simulation  of  a  deep-water  AUV  survey. 
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7.2  Future  Work 

The  decentralized  vehicle -based  algorithm  extends  naturally  to  multiple  vehicles — any 
vehicle  within  acoustic  range  of  the  ship  can  execute  its  own  independent  local  vehicle- 
based  filter.  A  natural  extension  of  the  decentralized  algorithm  is  to  incorporate  acoustic 
broadcasts  from  other  vehicles  in  addition  to  broadcasts  from  the  ship.  Inter-vehicle  range 
measurements  made  from  inter- vehicle  acoustic  broadcasts  would  further  constrain  the  nav¬ 
igation  solution  and  improve  each  vehicle’s  own  state  estimate.  However,  as  mentioned  in 
Section  6.4.3,  in  order  to  maintain  consistency  between  the  decentralized  filter  and  the  cen¬ 
tralized  filter  the  process  model  of  the  node  broadcasting  the  acoustic  data  (i.e.  the  ship) 
must  be  linear.  A  linear  process  model  ensures  that  measurement  updates  and  process  pre¬ 
dictions  in  the  ship  filter  are  independent  of  the  ship’s  current  state.  Thus,  the  calculation 
of  the  information  that  is  broadcast  from  the  ship  is  identical  to  that  of  the  centralized  filter, 
despite  the  fact  that  the  state  estimate  of  the  ship  from  the  centralized  filter  differs  from  the 
state  estimate  of  the  ship  from  the  independent  ship  filter.  (The  state  estimates  are  differ¬ 
ent  because  the  centralized  filter’s  estimate  is  conditioned  on  previous  range  measurements 
that  are  not  accessible  to  the  independent  ship  filter.  This  is  true  regardless  of  the  linearity 
of  the  ship  process  model.)  If  the  ship  process  model  is  not  linear,  measurement  updates 
and  process  predictions,  which  require  a  linearization  about  the  filter’s  estimated  current 
state,  will  be  different  in  the  centralized  filter  compared  to  the  independent  ship  filter.  So 
while  we  believe  that  assuming  a  linear  ship  process  model  is  both  reasonable  and  princi¬ 
pled,  the  vehicle  process  model  is  not  linear  as  defined  herein.  This  presents  a  challenge 
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for  extending  the  decentralized  algorithm  to  include  inter- vehicle  ranges.  In  addition,  the 
problem  of  over-confidence  associated  with  double-counting  information  passed  between 
the  vehicles  must  be  addressed. 
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Review  of  Single-Beacon 
Navigation  Literature 


This  appendix  provides  an  overview  of  the  selection  of  significant  papers  shown  in  Ta¬ 
ble  A.l  in  the  area  of  single  beacon  navigation  published  through  2006.  I  have  included  a 
summary  of  the  measurement  and  process  models  used  in  each  paper  as  well  as  a  summary 
of  the  authors’  conclusions.  Unless  noted  otherwise,  the  vehicle  coordinate  frame  is  as¬ 
sumed  to  be  defined  with  x  forward,  y  lateral  and  z  vertical,  and  the  body-frame  velocities 
u,  v  and  w  are  defined  in  the  x,  y  and  z  directions  respectively.  Thus  the  vehicle  body-frame 
velocity  u  is  in  the  direction  of  the  vehicle  heading. 
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Table  A.l:  Single -beacon  navigation  papers  reviewed  in  this  Appendix. 


Least  Squares  Approach 

A.  Scherbatyuk 

1995 

r  soi 

C.  Hartsfield 

2005 

L42J 

C.  LaPointe 

2006 

[571 

Least  Squares  Seeded  Extended  Kalman  Filter  Approach 

J.  Vaganay,  P.  Baccou,  B.  Jovencel 

2000 

[891 

P.  Baccou,  B.  Jovencel 

2002 

L3J 

P.  Baccou,  B.  Jovencel 

2003 

[4] 

Extended  Kalman  Filter  Approach 

M.  Larsen 

2000 

[58] 

M.  Larsen 

2001 

[59] 

M.  Larsen 

2002 

[60] 

Extended  Set-Valued  Observer  Approach 

J.  Manual,  J.  Jouffroy,  T.  Lossen 

2005 

[63] 

Observability  Analysis 

A.  Gadre,  D.  Stilwell 

2004 

[33] 

A.  Gadre,  D.  Stilwell 

2005 

[34] 

A.  Gadre,  D.  Stilwell 

2005 

[35] 

A.  Gadre 

2007 

[32] 

A.  Ross.  J.  Jouffroy 

2005 

[77] 

J.  Jouffroy,  J.  Reger 

2006 

[481 

T.  Song 

1999 

|85| 

B.  Ristic,  S.  Arulampalam,  J.  McCarthy 

2002 

L76J 

Multi-Beacon  Range-Only  SLAM 

P.  Newman,  J.  Leonard 

2003 

[71] 

E.  Olson,  J.  Leonard,  S.  Teller 

2006 

[72] 

G.  Kantor  and  S.  Singh 

2002 

[51] 

A.l  Least  Squares  Approach 


Scherbatyuk,  1995  [80] 

This  paper  is  the  earliest  formulation  of  the  single-beacon  navigation  problem  with 
unknown  currents  that  we  know  of.  The  author  proposes  to  use  least  squares  to  solve  for 
the  initial  vehicle  position  and  unknown  current  velocity.  The  author  also  reports  a  linear 
algebra-based  observability  analysis. 

Vehicle  Process  Model:  The  author  assumes  a  simple  kinematic  model  for  the  vehicle  in 
Cartesian  coordinates  in  which  the  vehicle  perfectly  maintains  a  piecewise  constant  heading 


114 


APPENDIX  A.  SINGLE-BEACON  NAVIGATION 


and  forward  velocity  through  the  water  using  dead  reckoning.  The  author  assumes  the 
vehicle  operates  at  a  constant  depth  with  no  side  slip,  i.e.  u  ^  0,  v  —  0,  w  —  0  in  body  frame 
coordinates.  The  vehicle’s  world  position  is  affected  only  by  the  unknown  current  and  its 
unknown  starting  position.  After  the  first  three  legs  when  the  the  first  current  estimate  is 
made,  this  estimate  is  used  in  subsequent  dead  reckoning  and  updated  after  each  leg. 

Observation  Model:  The  observation  model  is  also  formulated  in  Cartesian  coordinates. 
Process  inputs  are  range  to  the  single  beacon,  vehicle  yaw  (heading),  and  the  vehicle’s 
relative  velocity  through  the  water.  The  x,  y,  z  location  of  the  beacon  is  assumed  fixed  and 
known  and  the  vehicle  depth  is  assumed  known  without  noise,  reducing  the  problem  to  two 
dimensions.  Range  measurement  errors  are  modelled  as  symmetrical  mutually  independent 
uniform  random  variables  in  the  interval  [-0. 1,0.1].  The  vehicle  yaw  and  relative  velocity 
errors  are  modelled  as  mutually  independent  zero-mean  Gaussian  noise  with  dispersions 
2%  of  parameter  values.  All  of  these  quantities  are  assumed  known  at  each  time  step 
without  additional  measurement  noise. 

Method:  The  least  squares  method  is  used  to  determine  the  coefficients  that  arise  in 
the  quadratic  formulation  of  the  squared  range:  r2(k)  =  A(kr)2  +  B(kr )  +  C  where  k 
is  the  time  step,  r  is  the  time  step  interval,  r(k )  is  the  range  at  time  step  k  and  A,  B, 
and  C  are  unknown  coefficients  that  are  functions  of  the  vehicle  position,  heading  and 
velocity  and  the  beacon  position.  Once  the  coefficients  are  found  for  three  separate  legs, 
the  unknown  current  and  initial  vehicle  position  is  calculated  using  simple  linear  algebra 
techniques.  This  estimate  is  used  in  subsequent  dead  reckoning  estimates  and  updated  after 
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each  successive  leg  is  completed. 

The  statistical  characterization  of  the  evaluation  of  each  of  the  four  unknown  parameters 
(initial  vehicle  x,  y  position  and  x,  y  components  of  the  current)  was  explored  using  the 
Monte  Carlo  method. 

Observability:  The  author  reports  that  the  algorithm  requires  the  vehicle  to  traverse 
three  distinct  steady  straight-line  trajectories  in  order  to  uniquely  detect  the  current  velocity 
and  the  initial  vehicle  position.  Trajectories  directly  towards  or  away  from  the  beacon  are 
viable  trajectories. 

Hartsfield,  2005  [42] 

This  paper  presents  a  single-beacon  navigation  algorithm,  the  Single  Transponder  Range 
Only  Navigation  Geometry  (STRONG)  algorithm,  and  details  of  its  implementation  on  a 
REMUS  vehicle.  The  author  employs  an  iterative  technique  to  determine  vehicle  course 
(in  the  world  frame)  and  position  over  a  sequence  of  four  transponder-to- vehicle  ranges. 
Unknown  currents  are  permissible  but  are  not  estimated. 

Vehicle  Process  Model:  The  vehicle  process  model  is  formulated  in  Cartesian  coordi¬ 
nates.  For  the  least  squares  solution,  either  vehicle  velocity  or  heading  is  assumed  constant 
between  each  received  range  depending  on  whether  a  speed  or  course  correction  is  to  be 
done  (see  below).  The  effects  of  unknown  currents  are  assumed  to  be  subsumed  in  the 
constant  bias  error  of  the  vehicle  heading  and  velocity. 

Observation  Model:  The  observation  model  is  formulated  in  Cartesian  coordinates. 
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The  x,  y,  z  location  of  a  single  beacon  is  assumed  precisely  known,  as  well  as  the  vehicle 
depth,  resulting  in  a  two-dimensional  problem.  Ranges  between  the  transponder  and  the 
vehicle  are  measured  from  a  single  beacon  and  are  assumed  accurate  (no  measurement 
noise).  Variable-length  time  steps  are  defined  by  the  receipt  of  these  acoustic  ranges.  The 
body  frame  vehicle  velocity  with  respect  to  the  ground  is  known  from  a  Doppler  Velocity 
Log  (DVL).  Vehicle  heading  is  known  from  a  magnetic  compass. 

The  author  assumes  that  either  the  estimated  vehicle  velocity  or  the  estimated  vehicle 
heading  is  constant  between  successive  ranges.  Thus  the  estimated  vehicle  path  consist  of 
a  series  of  either  constant  bearing  or  constant  speed  path  segments,  as  described  below. 

Method:  The  author  uses  ad  hoc  iterative  techniques  to  assess  different  possible  veloc¬ 
ities,  headings  and  initial  positions  for  each  path  segment  independently.  Either  a  course 
correction  (heading  and  initial  position)  or  speed  correction  (velocity  and  initial  position) 
is  performed  depending  on  the  user’s  choice.  In  the  case  of  the  course  correction,  veloc¬ 
ity  is  assumed  to  be  measured  accurately  (no  measurement  noise  or  bias).  Heading  and 
initial  position  are  then  assessed  by  minimizing  the  squared  error  between  the  proposed 
path  and  the  four  most  recent  range  measurements.  The  process  iterates  between  head¬ 
ing  and  initial  position  until  a  solution  with  a  suitably  small  covariance  is  achieved.  The 
speed  correction  is  performed  similarly,  assuming  heading  is  measured  accurately  without 
measurement  noise  and  iterating  between  velocity  and  initial  position. 

Conclusions:  The  author  concludes  that  this  algorithm  is  a  viable  method  for  localizing 
a  REMUS  or  similarly  equipped  vehicle  using  ranges  from  a  single  beacon. 
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LaPointe,  2006  [57] 

This  paper  presents  a  single -beacon  navigation  algorithm,  the  Virtual  Long  Baseline 
(VLBL)  algorithm,  and  its  simulation.  The  author  proposes  a  method  for  time-aligning 
current  and  previous  range  fixes  and  uses  least  squares  to  solve  for  the  vehicle  position. 
The  robustness  of  the  method  with  respect  to  two  model  parameters  is  tested  in  simulation. 
The  effects  of  loss  of  observability  are  noted  where  manifested  in  the  simulated  results. 

Vehicle  Process  Model:  The  vehicle  process  and  noise  models  are  not  addressed  in 
detail,  except  to  note  that  the  vehicle’s  body-frame  velocity,  from  a  Doppler  velocity  log, 
heading,  from  a  magnetic  compass,  and  depth  are  known  inputs  to  the  model.  In  simulation 
the  author  assumes  a  deterministic  vehicle  model,  i.e.  no  process  noise,  such  that  the  vehicle 
follows  an  exact  path. 

Observation  Model:  The  observation  model  is  formulated  in  Cartesian  coordinates. 
The  measured  output  of  the  system  is  transponder- vehicle  ranges,  which  are  received  asyn¬ 
chronously.  The  transponder  is  assumed  to  be  at  a  fixed,  known  location.  Combined  with 
the  known  vehicle  depth  this  makes  the  problem  two-dimensional. 

Method:  The  VLBL  method  relies  on  interrogating  a  single  transponder  multiple  times 
and  “virtually”  advancing  each  transponder  along  the  estimated  vehicle  trackline  to  simu¬ 
late  multiple  simultaneous  transponder  fixes  from  different  locations. 

Simulations:  Simulations  were  performed  using  both  synthesized  data  and  real-world 
data  from  Autonomous  Benthic  Explorer  (ABE)  Dive  162.  The  author  nominally  uses  four 
transponder  ranges  for  the  least  squares  solution  of  the  vehicle  position.  For  the  simulated 
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data  set  no  noise  was  included.  Also,  the  navigation  fixes  are  plotted  as  calculated  with¬ 
out  filtering  or  processing.  Fixes  with  large  discontinuities  over  short  periods  of  time  are 
allowed  in  simulation. 

Obserx’ability:  The  author  notes  the  effects  of  loss  of  observability  as  indicated  by  the 
degradation  of  simulated  vehicle  navigation  fixes.  Degradation  in  vehicle  fixes  is  seen  at 
comers  in  the  survey  pattern  and  along  tracklines  with  a  nearly  constant  bearing  to  the 
transponder.  The  lack  of  global  observability  for  constant  heading  trajectories  as  manifest 
in  the  existence  of  a  parallel  indistinguishable  trackline  is  also  observable  in  the  simulated 
data. 

Range  Sampling  Rate:  The  author  assess  the  range  sampling  rate,  n,  for  its  effect  on 
the  performance  of  the  algorithm  using  simulated  data.  The  author  tests  four  values  of  n, 
(n  —  1,  n  —  4,  n  —  10,  n  —  25),  where  n  is  the  number  of  data  points  received  for  each 
sample  taken,  e.g.  for  n  =  4,  the  range  data  used  to  solve  for  the  vehicle  position  is  every 
4th  range  data  point  received.  Because  the  simulated  data  contains  no  noise,  the  author  is 
careful  to  point  out  that  the  experimental  results  can  potentially  significantly  overestimate 
the  observability  of  the  system,  especially  in  a  scenario  where  the  transponder  lies  very 
close  to  the  vehicle  trackline.  The  author  also  notes  the  need  to  balance  the  trade-offs 
between  faster  sampling  rates  (smaller  n’s),  which  could  render  the  least  squares  solution 
impossible  to  calculate  due  to  too  small  of  a  baseline  and  therefore  an  uninvertible  state 
matrix,  versus  slower  sampling  rates  (larger  n’s),  which  could  result  in  vehicle  position 
estimates  that  are  too  infrequent.  Simulations  using  real-world  data  show  the  effect  of 
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range  sampling  rate  and  the  degradation  that  occurs  at  slower  sampling  rates. 

Outlier  Rejections:  The  effect  of  varying  the  “outlier  rejection  factor”  was  tested  in 
simulation  using  real-world  data  from  ABE  Dive  162.  The  outlier  rejection  factor  repre¬ 
sents  a  radius  from  the  last  position  fix  outside  which  position  estimates  are  rejected.  The 
radius  is  calculated  based  on  a  percentage  of  the  expected  distance  travelled  based  on  dead 
reckoning.  The  tests  ranged  from  1.1  to  2.5  (i.e.  110%  to  250%  of  the  expected  distance 
travelled  from  the  last  navigation  fix)  and  showed  divergence  when  the  radius  was  either 
too  small  or  too  large. 

Conclusions:  The  author  concludes  that  the  VLBL  method  is  appropriate  as  a  redundant 
navigation  system  for  use  with  traditional  Long  baseline.  Future  work  suggested  by  the 
author  includes  automating  parameter  tuning  and  careful  transponder  location  selection  to 
ensure  observability  of  the  system. 

A.2  Least  Squares  Seeded 

Extended  Kalman  Filter  Approach 

Vaganay,  Baccou,  and  Jovencel,  2000  [89];  Baccou  and  Jovencel,  2002 
[3];  Baccou  and  Jovencel,  2003  [4] 

These  papers  present  a  method  for  implementing  single-beacon  navigation  using  Least 
Squares  to  generate  an  initial  position  estimate  for  the  vehicle  from  a  specific  trajectory 
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(a  circle)  and  an  extended  Kalman  filter  to  update  the  vehicle  position  estimate  over  time. 
The  authors  assume  the  presence  of  an  unknown,  constant  current  and  a  vehicle  without 
a  Doppler  Velocity  Log  (DVL)  sensor  (no  direct  speed-over-ground  measurements).  A 
two-vehicle  system  is  explored  in  [4]  that  is  in  essence  a  cascaded  version  of  the  original 
algorithm.  Results  of  simulations  using  simulated  data  and  real-world  data  are  presented. 
Observability  is  not  addressed. 

These  three  papers  address  successive  reports  by  the  same  authors  on  one  problem  and 
are  discussed  together  herein.  The  first  paper  provides  an  overview  of  the  methods,  which 
are  refined  for  the  second  and  third  papers.  Thus  only  the  methods  employed  for  [3]  and  [4] 
are  discussed  here. 

Vehicle  Process  Model:  The  vehicle  motion  is  simulated  using  a  kinematic  vehicle 
model  formulated  in  Cartesian  coordinates.  The  state  vector  for  the  model  is  defined  as 
s  =  [x,  y,  z,  vx,  vy,  du ],  where  x,  y.  z  are  the  vehicle  position  in  world  coordinates,  vx  and 
vy  are  the  x  and  y  components  of  the  the  current  (assumed  constant),  and  du  is  an  assumed 
constant  vehicle  speed  bias.  The  inputs  are  the  vehicle  heading  if:,  pitch  6,  and  speed 
through  the  water  u,  where  vehicle  velocity  through  the  water  is  derived  from  an  a  priori 
propeller  rpm  to  water  velocity  calibration.  A  DVL  is  not  used. 

Kalman  Filter:  The  extended  Kalman  filter  is  formulated  in  discrete  time  using  the 
vehicle  process  model.  The  state  noise  vector,  is  assumed  to  be  zero-mean  Gaussian 
with  a  covariance  matrix  of  Qk ■  The  authors  assume  that  the  x,y,z  position  of  the  beacon 
is  known  a  priori  without  error.  The  depth  of  the  vehicle  is  also  assumed  known  without 
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error,  making  the  problem  two-dimensional. 

The  only  observation  is  of  the  acoustic  ping’s  round-trip  time  of  flight,  T,  between  the 
vehicle  and  the  beacon.  T  is  a  function  of  the  vehicle  state  when  the  ping  was  sent,  the 
vehicle  state  when  it  was  received,  and  the  speed  of  sound.  The  measurement  noise,  Wk,  is 
assumed  to  be  zero-mean  Gaussian  with  a  variance  of  Rk. 

Method:  The  authors  use  a  two-part  approach  to  solving  the  problem  of  vehicle  homing 
in  the  presence  of  unknown  currents.  First,  least  squares  is  employed  to  calculate  an  initial 
estimate  of  the  vehicle  position  and  the  current  plus  vehicle  speed  bias  during  strategic 
vehicle  maneuvers.  Second,  a  Kalman  filter  is  then  used  to  continually  update  the  vehicle- 
beacon  relative  position  and  the  current  estimate  while  the  vehicle  is  homing  in  on  the 
beacon. 

LS  Initialization:  The  vehicle  is  commanded  to  maneuver  in  a  circle  collecting  range 
data  to  the  beacon.  After  pre-filtering  (see  [3]),  the  authors  select  fourteen  (14)  randomly 
selected  ranges  N  different  times  and  find  the  group  that  produces  the  smallest  median 
residual  (N  =  70  in  the  simulations).  Using  these  14  data  points,  the  system  of  equations 
for  the  vehicle  position  versus  range  and  current  are  then  solved  by  least  squares  using  the 
Levenburg-Marquardt  algorithm  to  estimate  the  vehicle’s  x-y  position  and  the  x,y  compo¬ 
nents  of  the  current.  To  calculate  an  initial  position  estimate  sufficiently  close  to  the  actual 
initial  position  and  thus  avoid  the  problem  of  the  existence  of  many  local  minima,  the  prob¬ 
lem  is  first  solved  for  position  only  (assuming  no  current).  That  position  is  then  used  as  an 
initial  estimate  for  the  least  squares  problem  with  an  unknown  current. 


122 


APPENDIX  A.  SINGLE-BEACON  NAVIGATION 


EKF  Homing:  The  non-linear  least  squares  solution  to  the  initialization  problem  was 
used  to  seed  the  Kalman  filter  for  the  homing  phase.  During  the  homing  phase  the  vehi¬ 
cle  uses  an  extended  Kalman  filter  to  update  the  beacon-vehicle  relative  position  estimate 
and  the  current  estimate.  The  vehicle  drives  in  a  spiral  pattern  in  order  to  minimize  the 
covariance  while  approaching  the  beacon. 

Simulations:  Baccou  and  Jovencel  in  [3]  test  the  above  method  in  simulation  and  also 
in  post-processing  with  real  vehicle  data  that  was  validated  by  LBL.  The  authors  report 
that  the  differences  between  LBL  and  their  method  “converge  to  zero”  despite  the  added 
perturbation  of  an  unmodelled  heading  bias.  The  authors  conclude  that  the  navigation 
method  is  “robust  and  efficient”  and  that  single -beacon  navigation  using  an  EKF  is  worthy 
of  further  study. 

Baccou  and  Jovencel  in  [4]  present  the  work  discussed  in  the  previous  paper  and  add  a 
simulation  of  two  vehicle  operations.  The  vehicles  are  assumed  to  act  in  a  leader/follower 
arrangement,  where  the  leader  knows  its  precise  position  (is  not  affected  by  currents,  etc.) 
and  the  follower  receives  periodic  range  and  state  information  from  the  leader  via  acoustic 
modem  link.  The  setup  is,  in  essence,  a  cascaded  one-vehicle,  single-beacon  problem 
where  each  vehicle  only  has  one  ranging  source.  The  difference  between  the  vehicles  is 
that  the  ranging  source  for  the  fist  vehicle  is  stationary  and  its  position  is  known  with 
certainty.  The  ranging  source  for  the  second  vehicle  is  the  first  vehicle,  which  is  moving 
and  in  reality  its  position  is  not  known  with  certainty,  though  the  algorithm  assumes  that  it 
is. 
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A.3  Extended  Kalman  Filter  Approach 

Larsen,  2000  [58];  Larsen,  2001  [59];  Larsen,  2002  [60] 

These  papers  present  the  implementation  of  a  single-beacon  navigation  solution  using 
an  error  state  Kalman  filter.  A  MARPOS®-aided  dead  reckoning  navigation  system  is  as¬ 
sumed,  which  includes  a  Doppler  velocity  log  for  body-frame  velocities  and  a  gyrocompass 
for  vehicle  heading.  The  author’s  thesis  [59]  contains  in-depth  information  about  the  con¬ 
cepts  presented  in  [58]  and  [60]  as  well  as  a  detailed  dynamic  model  of  vehicle  motion. 
The  author  briefly  discusses  the  use  of  Doppler  frequency  shift  in  addition  to  or  instead  of 
range  to  improve  dead  reckoning  position  estimation  in  [58],  but  does  not  pursue  the  idea. 

Vehicle  Process  Model:  The  author  assumes  a  MARPOS®  dead  reckoning  system, 
which  includes  a  Doppler  velocity  log  (DVL)  in  bottom-lock  mode  and  a  gyrocompass, 
providing  body-frame  vehicle  velocities  and  heading  respectively.  The  derivation  of  the 
dynamic  model  will  be  covered  only  in  a  most  cursory  manner  here.  The  reader  is  urged 
to  consult  [59]  for  significantly  more  detail.  Briefly,  the  dynamics  are  governed  by  8x  = 
Adx+Gbu,  where  the  state  vector,  5x,  is  comprised  of  various  inertial  navigation-dependent 
states  and  5u  is  comprised  of  inertial  navigation-related  angular  velocities  and  specific 
forces.  A  is  a  matrix  valued  coefficient  consisting  of  time-varying  functions  of  position, 
velocity,  attitude  and  specific  force.  G  is  also  a  matrix- valued  coefficient  that  maps  the  body 
frame  inertial  sensor  noise  into  the  navigation  frame.  See  [59]  for  a  complete  treatment  of 
the  definitions  and  derivations  of  these  quantities. 
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Kalman  Filter:  The  x,  y.  z  location  of  the  single  beacon  is  known  exactly.  Vehicle 
depth  is  also  assumed  to  be  known,  thus  reducing  the  problem  to  two  dimensions.  The 
author  assumes  a  random  walk  model  to  describe  x,  y  position  estimate  error  drift  from  the 
MARPOS®.  Range  to  the  beacon  is  measured  with  noise. 

An  error  state  Kalman  filter  in  Cartesian  coordinates  is  used  to  estimate  the  accrued 
dead  reckoning  error  state  (and  possibly  errors  in  the  beacon  x,  y  position  at  each  time 
step).  The  state  vector  is  the  error  state  vector,  dx  =  |(9dr,  0tp  where  6  dr  =  [8xv,  5yv],  the 
vehicle  position  error  from  dead  reckoning,  and  9tp  is  a  vector  of  the  measurement  errors 
of  each  range  measurement.  The  process  noise  is  assumed  to  be  zero-mean  Gaussian. 

For  the  observation  equation  of  the  Kalman  filter,  a  linearized  range  error  is  used  with 
the  addition  of  zero-mean  Gaussian  measurement  noise.  The  true  vehicle  and  beacon  posi¬ 
tions  are  approximated  by  their  corresponding  estimates  since  they  are  not  known.  Range 
measurement  noise  is  assumed  to  be  uncorrelated  white  Gaussian  noise  with  a  standard 
deviation  of  0.3  m. 

Observability:  Observability  is  not  directly  addressed.  The  author  notes  that  there  is 
some  asymmetry  in  the  position  accuracy  related  to  the  trajectory  and  suggests  a  trajectory 
perpendicular  to  the  bearing  of  the  transponder  for  best  results. 

Simulations:  The  results  from  two  simulations  are  presented. 

Survey  Grid  Simulation:  The  first  simulation  uses  real  data  from  a  600  m  by  600  m 
survey  grid.  The  vehicle  travels  at  a  speed  of  6  km/hr  (3.2  knots)  for  180  minutes  resulting 
in  ~  1 8.5  km  of  linear  travel.  A  single  simulated  beacon  is  placed  at  the  center  of  the  grid 


125 


APPENDIX  A.  SINGLE-BEACON  NAVIGATION 


with  an  initial  position  error  of  ~  11m  (5  m  N,  10  m  E).  Ranges  to  the  beacon  are  provided 
at  10-second  intervals. 

Survey  Line  Simulation:  The  second  simulation  uses  real  data  from  an  8km  long  linear 
track.  With  an  average  speed  of  roughly  6  km/hr  (3.2  knots)  the  trackline  took  approx¬ 
imately  80  minutes.  Two  simulated  transponders  are  placed  along  the  trackline  near  the 
beginning,  roughly  1.5  km  apart.  There  is  no  uncertainty  modelled  in  the  transponders  lo¬ 
cations.  The  transponders  are  interrogated  only  when  the  vehicle  is  within  600  meters  of 
them  at  a  rate  of  once  per  minute. 

Conclusions:  The  author  reports  that  in  the  survey  grid  simulation  the  position  error  at 
the  end  of  the  test  is  reduced  from  ~18  m  to  ~2  m  when  ranges  to  the  transponders  are 
included  in  the  position  estimate.  In  addition,  the  position  error  appears  to  converge  within 
the  first  few  minutes  despite  the  induced  error  in  the  initial  position  of  the  beacon.  In  the 
survey  line  simulation  the  position  error  at  the  end  of  the  test  is  reduced  from  ~18  m  to  ~5 
m  when  ranges  to  the  transponders  are  included  in  the  position  estimate. 

A.4  Extended  Set- Valued  Observer  Approach 

Mar^al,  Jouffroy,  and  Fossen,  2005  [63] 

These  papers  describe  the  implementation  of  an  extended  set-valued  observer  instead 
of  the  commonly  used  Kalman  filter  to  estimate  vehicle  position  using  ranges  from  a  sin¬ 
gle  beacon.  The  set-valued  estimator  assumes  the  presence  of  measurement  noise  that 
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is  bounded  but  not  necessarily  Gaussian,  in  contrast  to  the  Kalman  filter,  which  assumes 
Gaussian  noise.  According  to  the  authors,  the  impetus  behind  this  observer  choice  was  to 
provide  an  estimation  technique  that  would  provide  bounds  on  the  estimation  error  even  in 
the  presence  of  strong  non-linearities  in  model  and  non-Gaussian  noise.  The  output  of  the 
observer  is  not  as  “smooth”  as  the  Kalman  filter  but  it  is  reported  to  guarantee  that  the  true 
position  is  always  within  the  predicted  covariance  or  confidence  ellipsoid. 

Vehicle  Process  Model:  The  vehicle  model  has  state  variables,  in  polar  coordinates, 
of  [p(£)  Q(t)  ip(t)]  corresponding  to  the  vehicle’s  range  from  the  beacon,  bearing  from  the 
beacon,  and  heading.  Yaw  rate  and  velocity,  [r(t)  v(t)},  are  inputs;  range  and  heading,  [pit) 
are  measured. 

Obserx’ability:  The  authors  assess  the  observability  of  the  system  in  polar  coordinates 
by  proving  the  existence  of  an  admissible  control,  subject  to  certain  conditions,  in  order 
to  guarantee  the  distinguishability  of  two  non-identical  states  [75]  (reference  [17]  in  the 
paper).  The  authors  accomplish  this  using  the  Lie  derivatives  of  the  measurement  equation 
along  the  solutions  of  the  state  equation  (all  in  polar  coordinates).  They  show  that  a  tra¬ 
jectory  where  yaw  rate  r(t)  —  0  is  not  uniquely  distinguishable.  The  authors  also  discuss 
the  condition  when  yaw  rate  r(t)  is  small  leading  to  two  possible  trajectories  that  are  in 
close  spatial  proximity  to  each  other.  While  in  theory  the  trajectory  is  distinguishable,  the 
authors  claim  that  in  practice  the  measurement  noise  may  lead  to  the  observer  “settling” 
on  the  wrong  track,  especially  if  the  true  location  is  outside  of  the  3a-ellipsoid  confidence 
region  as  is  possible  with  the  EKF. 
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Set-Valued  Obsen’er:  The  authors  implement  the  observer  in  Cartesian  coordinates 
using  the  linear  matrix  inequalities  framework  as  described  in  [79]  and  [16]  (references 
[3],  [5]  in  the  paper).  The  sampling  dynamics  include  an  unknown  but  bounded  noise 
vector,  w,  multiplied  by  a  scaling  matrix,  B ,  for  the  state  equation.  The  measurement 
equation  also  includes  an  unknown  but  bounded  noise  vector  multiplied  by  a  scaling  matrix, 
v  and  I)  respectively.  The  prediction  step  is  defined  such  that  the  bounding  error  ellipsoid 
is  guaranteed  to  contain  the  true  vehicle  position  provided  proper  noise  and  linearization 
errors  are  chosen.  The  update  step,  similar  to  a  Kalman  filter,  provides  a  method  for  fusing 
information  with  different  statistics.  The  result  is  an  estimator  with  larger  error  bounds 
than  a  Kalman  filter  but  that  guarantees  the  inclusion  of  the  true  vehicle  position. 

Comparison  with  EKF:  The  observer  performance  was  compared  to  that  of  an  extended 
Kalman  filter  (EKF)  in  three  anecdotal  simulations  using  a  straight  line  trajectory,  a  “lawn¬ 
mowing”  trajectory,  and  a  “lawn-moving”  trajectory  with  bad  measurements  (measure¬ 
ments  with  non-Gaussian  error).  Note  that  despite  the  assumption  of  the  presence  of  not- 
necessarily-Gaussian  noise,  the  range  measurements  are  assumed  to  have  Gaussian  noise 
for  the  simulations.  In  particular,  the  authors  show  an  example  where  the  set- valued  ob¬ 
server’s  covariance  ellipsoid  contains  the  true  state  while  the  EKF’s  3-cr  covariance  ellip¬ 
soid  does  not.  The  relative  lack  of  smoothness  of  the  set-valued  observer  compared  to  the 
EKF  was  also  noted  by  the  authors. 
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A.5  Observability  Analysis 

Gadre  and  Stilwell,  2004  [33];  Gadre  and  Stilwell,  2005  [34];  Gadre  and 
Stilwell,  2005  [35];  Gadre,  2007  [32] 

These  papers  address  the  problem  of  navigation  from  range  measurements  in  the  pres¬ 
ence  of  either  known  or  unknown  currents.  A  novel  observability  analysis  for  linear  time- 
varying  systems  is  performed  for  the  system  with  no  current,  with  a  known  current,  and 
with  an  unknown  current.  The  authors  employ  limiting  systems  to  assess  uniform  ob¬ 
servability,  and  then  formulate  sufficient  conditions  for  the  existence  of  an  observer  with 
exponentially  decaying  estimation  error  for  the  cases  of  both  known  and  unknown  currents. 
The  performance  of  the  observers  is  demonstrated  using  real  field  data. 

Vehicle  Process  Model:  The  authors  use  a  kinematic  model  in  Cartesian  coordinates 
of  the  AUV,  assuming  that  the  vehicle  operates  at  a  constant  depth  with  no  side-slip,  i.e. 
u  7^  0,  v  =  0,  w  =  0  in  body-frame  coordinates.  The  authors  note  that  there  will  be  error 
between  the  estimated  and  the  true  motion  of  the  vehicle  during  turning  maneuvers  and 
when  the  change  in  currents  is  not  significantly  slower  than  the  estimation  time  constant. 
Vehicle  depth  and  transponder  depth  are  assumed  known;  thus  the  authors  solve  the  two- 
dimensional  problem  in  which  the  vehicle  and  the  transponder  are  in  the  same  horizontal 
plane.  Transponder  location  (x,y)  is  assumed  known  and  coincident  with  the  origin  of  the 
world  frame. 

In  the  case  of  known  or  zero  current,  the  state  vector  of  the  system  is  defined  by 
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s(t)  =  {x(t),y(t),ip(t)]T  where  x(t)  and  y(t)  are  the  vehicle  position  and  ip(t)  is  the 

vehicle  heading.  In  the  case  of  unknown  currents,  the  state  vector  is  defined  by  s(t)  = 
[x(t),y(t),(p(t),vx(t),vy(t)]T  where  vx{t)  and  vy(t)  are  the  x  and  y  components  of  the 
unknown  current.  Both  models  take  as  inputs  the  heading  rate  of  change  (from  a  MEMs 
gyrocompass)  and  vehicle  velocity  through  the  water  (based  on  a  pre-determined  mapping 
from  steady-state  propeller  angular  velocity).  The  measurement  vector  is  the  same  in  both 
cases,  h(t)  =  {\Jx2{t)  +  y2(t ),  (p{t)}T .  Output  measurements  are  made  via  magnetic  com¬ 
pass  for  vehicle  heading  and  acoustic  beacon  for  range. 

Observability:  For  all  three  systems  (no  current,  known  current,  unknown  current)  local 
observability  was  addressed  by  linearizing  the  system  about  arbitrary  potential  trajectories 
and  assessing  the  observability  of  the  linearized  time-varying  (LTV)  system. 

For  the  system  with  known  currents  (including  zero  current),  the  authors  use  the  stan¬ 
dard  rank  test  for  observability  of  LTV  systems  and  conclude  that  it  is  locally  observ¬ 
able  [33]  excluding  the  case  of  6  =  0  where  6  is  the  bearing  from  the  transponder  to  the 
vehicle.  Using  the  observability  grammian  of  a  subsystem,  Gadre  shows  that  the  system  is 
uniformly  observable  [32]  also  excluding  the  case  6  =  0. 

For  the  system  with  unknown  currents,  uniform  observability  is  shown  over  a  finite  time 
interval  of  length  5.  This,  combined  with  the  requirement  for  observability  over  all  sliding 
intervals  [t  —  5 ,  t]  in  \ta  +  A  t j] ,  proves  uniform  observability  for  5  over  the  finite  interval 

[to,  tf]. 

Using  this  result,  the  authors  formulate  a  specific  condition  on  the  vehicle  trajectory 
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that  is  sufficient  to  guarantee  the  existence  of  an  observer  with  asymptotically  stable  error 
dynamics — specifically  a  Luenberger  observer.  The  condition  shows  straight-line  trajecto¬ 
ries  that  do  not  pass  through  the  origin  to  be  observable.  Gadre  also  shows  in  his  thesis 
that  uniform  observability  over  a  finite  interval  is  sufficient  to  guarantee  the  existence  of  a 
stabilizing  Kalman  gain  over  a  finite  interval  [32]. 

All  of  these  observability  results  are  local  and  depend  on  the  estimated  initial  state  being 
sufficiently  close  to  the  actual  initial  state.  For  example,  straight  line  trajectories  are  locally 
observable  but  not  globally  observable  due  to  the  existence  of  a  parallel  indistinguishable 
trajectory.  Straight  line  trajectories  that  pass  through  the  origin  are  not  observable,  but 
trajectories  including  but  not  exclusively  consisting  of  a  trajectory  whose  extension  passes 
through  the  origin  are  observable. 

The  authors  also  simulate  the  effect  of  a  non-zero  angle  of  attack  such  as  is  present  dur¬ 
ing  turning  maneuvers  and  changing  currents.  The  authors  conclude  that  bounded  estima¬ 
tion  error  will  occur  but  exponentially  decay  towards  zero  during  straight  line  trajectories. 

Kalman  Filter:  Although  a  Luenberger  observer  is  used  for  the  stability  analysis,  a 
discrete-time  extended  Kalman  filter  in  Cartesian  coordinates  is  used  for  the  implementa¬ 
tion  of  the  observer  in  post-processing  simulations.  The  authors  assume  that  the  vehicle 
velocity  is  parallel  to  its  heading  (i.e.  a  no-slip  condition).  The  implementation  is  referred 
to  as  “standard”  by  the  authors  except  for  the  process  noise  covariance  matrix,  which  varies 
with  time  to  allow  for  a  larger  process  noise  during  turning  maneuvers. 

Testing:  Testing  was  carried  out  using  the  Virginia  Tech  (VT)  Miniature  AUV  in  a 
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lake  for  the  system  with  no  current  and  on  a  river  for  the  systems  with  known  or  unknown 
currents.  The  vehicle  successfully  localized  its  position  in  the  presence  of  both  zero  and 
known  non- zero  current.  The  vehicle  successfully  localized  its  position  and  estimated  the 
current  in  the  presence  of  unknown  and  varying  currents.  The  authors  conclude  that  slowly 
varying  unknown  currents  introduce  negligible  errors;  therefore  this  algorithm  is  suitable 
for  real-time  analysis. 

Ross  and  Jouffroy,  2005  [77] 

This  paper  provides  a  concise  treatment  of  the  observability  of  a  single-beacon,  single¬ 
vehicle  measurement  system  in  continuous  time.  The  authors  use  Lie  derivatives  to  com¬ 
pute  the  conditions  for  which  the  system  has  local  weak  observability. 

Vehicle  Process  Model:  The  kinematic  vehicle  model  is  formulated  in  continuous  time 
in  both  Cartesian  and  polar  coordinates.  In  Cartesian  coordinates  the  model  has  a  state 
vector  of  [x(t),y(t)],  the  vehicle’s  horizontal  position.  Inputs  to  the  system  are  fixed  body 
velocities,  u,  v,  from  a  bottom-lock  Doppler  sonar,  and  heading,  0,  from  a  gyrocompass. 
The  measurement  vector  consists  of  the  range  to  the  vehicle  from  the  beacon  (assumed  to 
be  at  the  origin  of  the  world  frame). 

Observability:  In  polar  coordinates,  using  an  analysis  of  the  Lie  derivatives  of  the  out¬ 
put  function  and  the  observability  rank  condition  for  nonlinear  systems  [43]  (reference  [6] 
in  the  authors’  paper),  the  authors  show  that  the  system  is  locally  weakly  observable  pro¬ 
vided  that  the  vehicle  is  not  travelling  directly  towards  or  away  from  the  beacon.  Further, 
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for  any  trajectory  to  be  distinct  the  vehicle  must  change  course  at  least  once  during  the 
trajectory. 

Kalman  Filter:  The  authors  propose  an  observer  based  on  a  modified  EKF  to  give 
a  prescribed  degree  of  stability  as  presented  in  [49].  The  state  vector  of  the  observer  is 
z(t)  =  | fiff ),  7(4)]  '  where  Bit)  and  7  are  the  vehicle’s  range  and  bearing  to  the  beacon 
(the  origin  in  this  analysis).  The  measurement  vector  is  simply  the  first  entry  in  the  state 
vector,  R(t),  the  range.  Process  and  measurement  noise  characteristics  are  not  mentioned. 

Simulations:  Results  are  presented  from  four  simulations:  a  straight-line  trajectory  with 
good  initialization  where  the  observer  converges;  a  straight-line  trajectory  with  bad  initial¬ 
ization  where  the  observer  converges  on  the  parallel  indistinguishable  trajectory;  a  maneu¬ 
vering  trajectory  where  the  observer  initially  converges  on  the  parallel  indistinguishable 
trajectory  and  then  after  the  maneuver  onto  the  correct  trajectory;  and  finally  an  unobserv¬ 
able  trajectory  where  the  observer  fails  to  converge. 

Jouffroy  and  Reger,  2006  [48] 

The  authors  address  the  observability  of  a  single-vehicle,  single-beacon  system  using 
what  the  authors  call  an  “algebraic  method”,  by  expressing  the  state  as  a  function  of  the 
input,  the  measured  output,  and  a  finite  number  of  their  derivatives  to  show  local  uniform 
observability.  The  authors  also  present  an  “algebraic  estimator”  that  relies  on  fitting  a 
second  order  polynomial  to  the  signal  over  a  sliding  time  window.  The  estimated  signal 
and  its  derivatives  are  used  with  the  proposed  observability  mapping  to  estimate  the  state 
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of  the  system.  This  technique  lacks  an  estimation  model  however,  which  disallows  the 
computation  of  an  updated  position  without  a  measurement. 

Vehicle  Process  Model:  The  system  model,  in  polar  coordinates,  has  a  state  vector  of 
[/?(£),  7(f)]T,  the  range  and  bearing  from  the  beacon  to  the  vehicle.  Inputs  to  the  system  are 
u  and  v,  the  vehicle  velocity  in  body-frame  coordinates  from  bottom-lock  Doppler  mea¬ 
surements,  and  ip,  the  vehicle  heading  measured  by  a  gyrocompass.  The  angular  velocity 
ip  =  r  is  intentionally  omitted  by  the  authors  because  it  is  assumed  measured  by  an  Inertial 
Measurement  Unit  (IMU).  The  measurement  vector,  though  not  explicitly  defined,  consists 
of  the  beacon-vehicle  range,  R(t). 

Observability:  The  authors  assess  the  observability  of  the  system  in  polar  coordinates 
using  an  algebraic  approach  developed  by  [21]  (reference  [1]  in  the  paper).  The  authors 
analyze  the  conditions  for  which  the  state  can  be  expressed  as  a  function  of  the  input, 
the  measured  output,  and  a  finite  number  of  their  derivatives,  which  is  equivalent  to  local 
uniform  observability.  The  authors  show  that  the  system  is  not  globally  observable  for 
constant  heading  trajectories  because  every  straight  line  trajectory  that  does  not  extend 
through  the  beacon  location  has  a  parallel  indistinguishable  trajectory.  They  also  show  that 
the  system  is  unobservable  for  trajectories  directly  towards  or  away  from  the  beacon  and 
when  the  vehicle  velocity  is  zero,  p  =  \Ju2  +  v2  =  0. 

Estimation  Technique:  The  authors  use  time-derivative  estimation  techniques  from  [46] 
and  [47]  (references  [9],  [10]  in  the  paper)  to  estimate  a  polynomial  representation  of  the 
desired  signal  using  multiple  integrations  of  the  desired  signal.  The  derivatives  of  the  signal 
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are  estimated  by  simply  differentiating  the  polynomial  estimation.  The  authors  point  out 
that  multiple  integrals  of  the  signal  are  “readily  and  practically  available... since  integrators 
will  augment  the  signal/noise  ratio.”  In  order  to  ease  the  computational  burden  of  perform¬ 
ing  multiple  time  integrals  at  every  time  step,  the  authors  suggest  utilizing  the  differential 
relationship  between  the  time  integrals. 

To  address  the  problem  of  fitting  a  second  order  polynomial  to  a  real  signal  over  con¬ 
stantly  increasing  time  scale  (0  to  t ),  the  authors  suggest  fitting  a  polynomial  over  a  fixed 
length  time  interval  (t  —  T  to  t).  The  authors  re-derive  the  time-derivative  estimation  tech¬ 
nique  for  this  sliding  time  window  assumption. 

Finally,  the  authors  combine  the  derived  estimation  technique  and  the  conditions  on 
observability  to  create  an  “algebraic  estimator”  for  R(t)  and  7 (t).  In  the  estimator,  pit)  and 
o(t)  (from  the  Doppler  sensor)  and  u>(t)  are  all  assumed  to  be  corrupted  by  measurement 
noise,  the  nature  of  which  is  not  defined.  The  range  measurement  R(t)  is  also  assumed  to 
be  corrupted  by  Gaussian  white  noise. 

Simulations:  Two  400  second-long  simulations  are  conducted  using  the  proposed  es¬ 
timator:  a  “well-behaved”  straight  trajectory  that  moves  the  vehicle  past  the  beacon  and 
a  straight  trajectory  that  moves  the  vehicle  directly  away  from  the  beacon.  In  the  results 
from  the  first  simulation  the  estimator  appears  to  converge  on  the  actual  vehicle  state  after 
about  50  seconds  of  transient  behavior.  The  results  from  the  second  simulation,  according 
to  the  text,  show  that  the  curves  are  very  noisy,  but  the  related  figure  does  not  appear  to 
reflect  the  stated  simulated  vehicle  path  or  the  stated  results,  i.e.  the  figure  does  not  show 
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7  =  constant  and  R  =  u  as  one  would  expect  for  a  vehicle  on  a  constant-bearing  trajectory 
from  the  beacon. 

Future  work  suggested,  but  not  pursued  by  the  authors,  includes  taking  into  account 
the  time  delays  inherent  in  acoustic  systems  as  well  as  different  update  rates  for  different 
inputs.  An  improvement  in  noise  reduction  is  also  suggested. 

Song,  1999  [85] 

This  paper  addresses  target  tracking  using  only  range  measurements  in  the  which  the 
target  is  assumed  to  be  moving  with  constant  acceleration.  The  problem  set-up  differs  from 
the  vehicle  navigation  papers  that  model  a  vehicle  estimating  its  own  position  from  range 
measurements.  Here  an  observer,  the  tracker,  is  estimating  the  position  of  a  target  relative 
to  itself,  using  only  target-tracker  range  information.  The  heading  of  the  target  is  not  used. 
The  author  derives  necessary  and  sufficient  conditions  for  local  system  observability  using 
the  Fisher  information  matrix  in  “modified  polar  coordinates.” 

Target  Process  Model:  The  target  is  modeled  as  a  dynamic  system  in  continuous  time 
using  Cartesian  coordinates.  The  state  vector  is  x  =  [A",  Y.  X ,  Y .  ATx ,  ATy ] ,  where  X  and 
Y  are  the  target  position  relative  to  the  tracker,  X  and  Y  are  the  relative  velocity,  and  Arx 
and  ATy  are  the  assumed  constant  world  frame  accelerations  of  the  target.  The  input  is 
Am  =  (Arnx ,  Arnx),  the  tracker  acceleration  vector.  The  author  assumes  that  the  dynamics 
are  modelled  perfectly  with  no  process  noise. 

Observation  Model:  Measurements  are  taken  in  polar  coordinates  at  discrete  times. 
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The  only  measurement  in  the  system,  z(t),  is  the  tracker-target  range,  r(t).  The  author 
assumes  zero-mean  Gaussian  measurement  noise,  v(t),  with  variance  a2  such  that  z(U)  = 
r(ti )  +  v(ti)  at  discrete  time  t  —  U. 

Obsen’ability:  The  observability  analysis  first  requires  a  reformulation  of  the  state  vec¬ 
tor  in  polar  coordinates.  The  state  vector  is  redefined  as  y(t)  =  [/3,  f/r,  (3, 1/r,  ATf3,ATr] 
where  (3  is  the  target-tracker  bearing,  r  is  the  target-tracker  range,  and  /1T;J  and  ATr  are  the 
bearing  and  range  accelerations  respectively.  The  observability  criteria  are  derived  from  the 
requirement  that  the  Fisher  information  matrix  be  positive  definite  and  result  in  a  necessary 
and  sufficient  condition  for  local  observability. 

This  observability  condition  dictates  that  tracker/target  trajectories  resulting  in  con¬ 
stant  relative  bearing  are  globally  unobservable.  More  specifically,  when  the  target  is  on  a 
course  directly  towards  or  away  from  the  tracker  (in  the  tracker’s  frame)  the  target’s  course 
is  globally  and  locally  unobservable.  Also,  when  the  the  target  and  tracker  both  have  con¬ 
stant  bearing  (regardless  of  direction)  and  constant  acceleration  (including  zero),  the  target 
has  a  linear  trajectory  with  respect  to  the  tracker,  resulting  in  a  locally  observable  but  not 
globally  observable  trajectory  (i.e,  a  parallel  indistinguishable  trajectory  exists).  The  au¬ 
thor  suggests  that  the  tracker’s  trajectory  should  contain  non-zero  jerk  in  order  to  track  a 
target  with  constant  acceleration  and  a  non-zero  acceleration  to  track  a  target  with  constant 
velocity. 

Simulation:  The  author  uses  an  extended  Kalman  filter  in  Cartesian  coordinates  with 
the  state  and  observation  models  discussed  above.  A  0.2  Hz  update  rate  and  a  measurement 
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noise  with  cr2  =  0.1m2  are  used  for  simulations.  The  structure  of  the  EKF  is  not  discussed 
in  detail.  The  author  reports  that  the  simulation  results  “indicate  good  tracking  performance 
for  target  states  including  position,  velocity,  and  acceleration.” 

Ristic,  Arulampalam,  and  McCarthy,  2002  [76] 

The  authors  make  several  limiting  assumptions  concerning  the  problem  statement  and 
the  dynamic  models  of  the  target  and  tracker  as  detailed  below.  They  then  compute  the 
theoretical  Cramer-Rao  lower  bound  and  compare  that  to  the  results  from  three  different 
estimators  using  Monte  Carlo  simulations.  The  three  estimators  analyzed  are  a  maximum 
likelihood  estimator,  an  angle-parameterized  extended  Kalman  filter,  and  a  regularized  par¬ 
ticle  filter. 

The  authors  make  several  limiting  assumptions  for  the  dynamic  model  of  the  system. 
The  authors  assume  that  the  target  is  on  a  constant  velocity,  constant  bearing  trajectory. 
The  target  kinematic  state  is  defined  as  sk  =  [xtk,xtk,ytk,  y'k]  .  The  observer  state  is  defined 
as  s°k  =  [x°k,  x°k,  y°k ,  yk}T.  Thus  the  relative  state  vector  is  sk  =  -  s°k  =  [xk,  xk,  yk,  yk]T . 

The  authors  further  assume  that  the  target’s  initial  position  at  time  t0  is  known  and  that 
the  tracker  is  on  a  constant  velocity  circular  trajectory.  Range  measurements  to  the  target 
are  made  at  regular  intervals,  Ts,  starting  at  some  time  delay,  Td,  after  the  initial  detection, 
t0,  where  Td  »  Ts.  This  represents  the  target  position  acquisition  by  the  Ingara  radar  in 
Scan  mode  at  time  t0,  the  time  delay,  Td,  to  switch  the  instrument  to  ISAR  mode,  and  then 
regular  range  measurements  at  intervals  of  Ts  thereafter.  Because  of  these  assumptions,  I 
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believe  that  the  observability  analysis  and  the  estimation  method  comparison  may  not  be 
applicable  to  single -beacon  OWTT  navigation. 

A.6  Simultaneous  Localization  and  Mapping: 

Navigating  with  range-only  measurements  to  multiple  beacons 
with  no  a  priori  knowledge  of  beacon  location 
Newman  and  J.  Leonard,  2003  [71] 

This  paper  addresses  the  topic  of  range-only  navigation  using  multiple  acoustic  beacons 
but  without  the  use  of  any  additional  navigation  aids  (i.e.  no  Doppler  velocity  log  or  inertial 
measurement  unit)  and  without  a  priori  knowledge  of  the  location  of  the  acoustic  navigation 
beacons.  The  authors  use  a  non-linear  least  squares  method  to  solve  for  a  concatenation  of 
the  last  N  vehicle  positions  and  the  beacon  locations  and  present  simulation  results  based 
on  data  collected  in  the  field. 

Process/T rajectory  Model:  The  state  vector  of  the  system  consists  of  some  predeter¬ 
mined  number,  N,  of  the  most  recent  vehicle  pose  states  as  well  as  the  location  of  i  acoustic 
beacons. 

X  •••  *  *  *  ,  ] 

In  place  of  an  explicit  process  model,  the  authors  use  a  trajectory  function,  /(X)  =  0, 
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to  constrain  the  trajectory  of  the  vehicle  to  a  constant  velocity.  Vehicle  acceleration  is 
modelled  as  a  zero-mean  Gaussian  random  variable,  x(-)  ~  A/"(0,  Q).  This  leads  to  a  linear 
relationship  between  any  three  consecutive  vehicle  positions:  £(.)-!  —  ^x{-)  +  x(  )+i  =  0- 

Observation  Model:  The  acoustic  time  of  flight  between  the  vehicle  and  each  of  the  ith 
beacons  is  concatenated  into  a  measurement  vector  Zt  =  /i(X).  The  covariance  of  Zt,  R, 
is  a  diagonal  matrix  consisting  of  the  variance  of  each  observation.  The  authors  model  the 
observation  error  as  a  Cauchy  distribution  with  half  maximum  width  a. 

Least  Squares  Solution:  The  combination  of  observations  and  constraint  equations  is 
solved  using  a  non-linear  least-squares  approach.  Z  is  the  stacked  vector  consisting  of  the 
observations  and  the  trajectory  constraints. 


’  Zt ' 

"  MX) ' 

0 

.  /(X)  _ 

X  =  h{X) 

Given  Z,  the  authors  use  the  large-scale  non-linear  optimization  method  presented  in 
[41]  to  solve  the  linearized  system  of  Gauss-Newton  equations,  H Sx  =  JTW<5z,  where 
H  is  the  Hessian  and  J  is  the  Jacobian  of  Z  evaluated  at  the  current  X.  The  measurement 
residual  is  5z  =  Z  —  /i(X)  and  W  is  the  observation  weight  matrix,  W  =  diag(R _1,  Q-1). 

Experiments:  Data  was  collected  for  simulation  during  an  experiment  in  shallow  water 
(~1  lm  depth)  using  an  Odyssey  III  class  vehicle.  Acoustic  beacons  were  dropped  onto  the 
seafloor  and  their  surface  release  positions  marked  with  a  DGPS.  These  beacons  were  used 
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to  independently  calculate  the  absolute  position  of  the  vehicle  and  compared  to  the  results 
of  the  algorithm.  The  vehicle  navigated  using  an  extended  Kalman  filter  (EKF)  with  LBL 
(and  a  priori  knowledge  of  the  beacon  locations)  as  well  as  compass  and  Doppler  velocity. 
The  survey  consisted  of  parallel  tracklines  approximately  75  m  apart  and  350  m  long. 

Due  to  a  synthetic  aperture  sonar  on  the  vehicle  whose  frequency  overlaps  the  frequency 
band  of  the  acoustic  beacons,  the  range  measurement  data  were  extremely  noisy  with  a  sig¬ 
nificant  number  of  outliers  and  non-Gaussian  noise.  To  counter  this,  the  data  were  filtered 
to  reject  outliers  using  a  series  of  ad  hoc  algorithms.  The  filtered  range  information  for 
approximately  1  km  of  the  survey  was  used  in  post-processing  as  input  to  the  range-only 
algorithm. 

Results:  Due  to  the  nature  of  the  range-only  navigation  problem  presented,  i.e.  a  so¬ 
lution  found  without  any  additional  navigation  aids,  the  range-only  navigation  solution 
produced  is  relative  to  the  beacons  without  a  world-frame  reference.  Thus  the  solution  is 
unconstrained  in  x,  y  and  6  and  must  be  rotated  and  translated  in  order  to  compare  it  to 
the  EKF  solution.  The  authors  accomplish  this  by  choosing  a  rotation  and  translation  that 
collocates  the  position  of  the  transponder  labelled  “Tl”  and  aligns  the  baseline  between 
T1  and  T2.  The  results  show  baseline  length  errors  between  1.3%  and  2.3%  with  trackline 
following  within  5-10  m  of  the  EKF  solution  except  when  the  trackline  was  in  a  geomet¬ 
rically  undesirable  position  with  respect  to  the  acoustic  beacons.  During  these  periods  the 
off-track  error  between  the  EKF  and  the  range-only  solution  deteriorated  to  20-30  m. 
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Olson,  Leonard,  and  Teller,  2006  [72] 

This  paper  addresses  vehicle  position  estimation  using  only  range  measurements  to 
multiple  beacons  without  a  priori  knowledge  of  beacon  locations.  The  main  result  pre¬ 
sented  in  this  paper  is  an  outlier  rejection  algorithm  for  extremely  noisy  range  data  with 
a  large  number  of  non-Gaussian  outliers.  The  authors  also  present  an  algorithm  for  the 
determination  of  the  initial  estimate  of  a  beacon’s  position,  in  this  case  to  seed  an  extended 
Kalman  filter  (EKF).  Finally,  the  authors  present  the  optimal  vehicle  trajectory  for  disam¬ 
biguating  between  two  possible  beacon  locations.  The  authors  use  an  EKF  to  estimate 
vehicle  and  beacon  locations  and  they  present  a  method  for  dynamically  increasing  the  size 
of  the  state  vector  (and  covariance  matrix)  as  new  beacons  are  identified  and  initialized. 

Pre-Filtering  Range  Data:  The  authors  present  an  outlier  rejection  algorithm  using  a 
form  of  spectral  graph  partitioning  denoted  Single  Cluster  Graph  Partitioning  (SCGP).  This 
algorithm  relies  solely  on  pairs  of  concurrent  range  measurements  and  vehicle  position 
estimates  requiring  no  a  priori  knowledge  of  beacon  locations.  This  review  will  not  cover 
the  outlier  rejection  algorithm  in  detail  because  it  is  not  the  focus  of  the  review,  but  readers 
interested  in  more  information  about  the  algorithm  are  encouraged  to  read  both  the  paper 
reviewed  here  [72]  and  the  previous  paper  published  by  the  authors  on  the  subject  [73]. 

Process  Model:  The  process  model  is  not  explicitly  addressed  except  to  define  the  state 
vector  xn  =  [rx,  ry,rt,bx,  by],  where  rx,  ry,  r,  are  the  vehicle  x,y  position  and  heading  in 
world  coordinates  respectively,  and  bx  and  by  are  a  beacon  location  in  world  coordinates 
(one  ( bz,by )  pair  exists  for  every  beacon).  As  new  beacons  are  detected  and  initialized, 
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they  are  added  to  the  state  vector  and  the  state  covariance  matrix  is  updated.  Neither  the 
authors’  assumptions  concerning  vehicle  dynamics  nor  the  existence  or  characteristics  of 
process  noise  are  reported  in  the  paper. 

Observation  Model:  The  observation  model  is  not  explicitly  defined.  However,  the 
estimated  measurement,  as  calculated  from  the  current  state,  is  defined  as  zn  =  \(rx  - 
bx)2  +  (ry  —  by)2]1/'2.  The  existence  of  measurement  noise  is  not  addressed  in  the  paper. 

Extended  Kalman  Filter:  To  estimate  vehicle  and  beacon  positions,  the  authors  imple¬ 
ment  an  extended  Kalman  filter  with  the  state  vector  defined  above.  The  authors  propose  an 
algorithm  for  obtaining  the  initial  estimate  of  a  beacon  position  by  comparing  every  possi¬ 
ble  pair  of  data  points,  where  a  data  point  consists  of  a  vehicle  location  and  a  range  to  the 
beacon.  Using  a  “voting  scheme”,  each  intersection  of  possible  beacon  locations  among  all 
of  the  pairs  of  data  points  is  recorded  as  a  “vote”  for  that  location.  As  soon  as  the  difference 
in  the  number  of  votes  between  any  two  locations  exceeds  a  preset  number  of  votes,  the 
location  with  the  most  votes  is  chosen  as  the  approximate  beacon  location  used  to  seed  the 
extended  Kalman  filter. 

During  this  voting  process,  two  distinct  candidate  beacon  positions  may  appear  to  be 
equally  likely.  The  authors  present  a  simple  calculation  based  on  the  gradient  of  the  ab¬ 
solute  difference  in  range  between  the  two  candidate  positions  to  determine  the  optimal 
vehicle  trajectory  to  disambiguate  the  true  beacon  position. 

Experiments:  Data  used  for  simulation  was  collected  during  an  experiment  in  shallow 
water  (~llm  depth)  using  an  Odyssey  III  class  vehicle.  Acoustic  beacons  were  dropped 
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onto  the  seafloor  and  their  surface  release  positions  marked  with  a  DGPS.  These  positions 
were  used  as  ground  truth  for  the  algorithm.  The  vehicle  navigated  using  an  EKF  with  LBL 
(and  a  priori  knowledge  of  the  beacon  locations)  as  well  as  compass  and  Doppler  velocity. 
The  survey  consisted  of  parallel  tracklines  approximately  75  m  apart  and  350  m  long.  This 
is  the  same  data  set  used  by  Newman  and  J.  Leonard  in  [71]. 

Due  to  a  synthetic  aperture  sonar  whose  frequency  overlaps  the  frequency  band  of  the 
acoustic  beacons,  the  range  measurement  data  were  extremely  noisy  with  a  significant  num¬ 
ber  of  outliers  and  non-Gaussian  noise. 

Navigation  Results:  Because  the  navigation  solution  is  calculated  only  relative  to  the 
beacons  without  a  world  frame  reference,  the  solution  is  unconstrained  in  x,  y  and  6  and 
must  be  rotated  and  translated  in  order  to  compare  it  to  the  EKF  solution.  The  authors 
used  one  beacon  to  determine  the  global  alignment  and  found  errors  for  the  remaining 
three  beacon  positions  between  1.5m  and  3m.  The  authors  note  that  because  a  ground  truth 
beacon  position  was  used  to  determine  the  global  alignment,  the  beacon  position  error  may 
be  underestimated. 

Kantor  and  Singh,  2002  [51] 

This  paper  presents  several  estimation  methods  applied  to  the  problem  of  robot  localiza¬ 
tion  using  range  measurements  from  multiple  beacons.  The  authors  address  three  separate 
topics  in  mobile  robot  localization:  static  localization,  position  tracking,  and  simultaneous 
localization  and  tracking. 
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Static  localization  is  accomplished  using  Markovian  probability  grid  methods  and  tested 
experimentally.  For  position  tracking,  the  authors  compare  simulation  results  from  an  ex¬ 
tended  Kalman  filter  (EKF)  and  a  Monte  Carlo  particle  filter.  For  both  the  static  localization 
and  the  position  tracking,  the  authors  assume  that  the  beacon  locations  are  known  precisely. 
Finally,  assuming  that  precise  beacon  locations  are  not  known  a  priori,  the  authors  present 
simulation  results  for  a  simultaneous  localization  and  mapping  (SLAM)  algorithm  that  uti¬ 
lizes  an  extended  Kalman  filter. 

Static  Localization  with  Probability  Grids:  To  test  static  localization,  the  authors  use  a 
Markovian  probability  grid  method.  ETsing  a  50’x50’  test  area  divided  into  l’xl’  squares, 
each  square  is  assigned  a  real  number,  Ps,  equal  to  the  probability  that  the  robot  is  occupy¬ 
ing  that  square.  Given  a  measurement  from  a  beacon,  m*,  and  that  beacon’s  location,  X},,  the 
probability  for  each  square  is  calculated  by  Ps  =  p{rs\nrii)/{2'Krs(3),  where  rs  =  \\xb~  xs|| 
and  xs  is  the  location  of  the  center  of  the  square.  T  is  a  constant  normalizing  factor  to 
ensure  that  the  total  probability  over  the  test  area  adds  to  1 . 

Given  a  probability  grid  for  every  measurement  from  each  beacon,  the  authors  com¬ 
bine  the  probability  grids  using  point- wise  multiplication  and  re-normalize.  This  combined 
probability  grid  is  then  used  to  calculate  a  weighted  average  of  the  estimated  vehicle  posi¬ 
tion  and  covariance. 

Experimentally,  the  authors  selected  approximately  100  different  vehicle  locations  dis¬ 
tributed  across  a  50’x50’  test  area  with  eight  radio-frequency  beacons.  Each  radio  fre¬ 
quency  beacon  provides  range  measurements  in  feet  discretized  to  a  values  in  the  set 


145 


APPENDIX  A.  SINGLE-BEACON  NAVIGATION 


{0,6,12,18,25,31,37,43,50}.  The  authors  experimentally  determined  the  probability  den¬ 
sity  function  for  each  measurement. 

Using  the  probability  grid  method  of  localization,  the  authors  found  an  average  estima¬ 
tion  error  of  1.62  feet. 

Position  Tracking:  For  the  purpose  of  position  tracking  the  authors  compare  an  ex¬ 
tended  Kalman  filter  (EKF)  and  a  Monte  Carlo  particle  filter. 

Extended  Kalman  Filter:  The  EKF  is  based  on  the  discrete-time  process  model 


x(k  +  1)  =  x(k)  +  T 


ui(k) 

u2(k) 


+  w(k), 


where  ui(k),u2(k)  are  the  vehicle  x  and  y  velocities  in  world  coordinates  and  w(k)  is  a 
zero-mean,  independent  and  identically  distributed  Gaussian  random  vector  with  covari¬ 
ance  R.  The  static  solution  described  above  is  used  as  the  seed  for  the  EKF. 

The  authors  create  an  ad  hoc  Gaussian  distribution  to  represent  a  pseudo-measurement 
of  vehicle  position.  The  pseudo-measurement  is  based  on  the  actual  range  measurement 
such  that  the  vehicle  position  lies  at  the  measured  range  and  the  angle  to  the  beacon  is 
based  on  the  prior: 

z  =  xb  + 


rm  cos  9 
fm  sin  9 


where  fm  is  the  mean  of  the  range  measurement  and  9  is  the  angle  between  the  x  axis  and 
the  line  between  the  beacon  and  the  prior  estimated  vehicle  location.  Instead  of  modelling 
measurement  noise,  the  authors  assign  a  covariance  matrix  C  to  the  pseudo-measurement 
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such  that 

<hT, 

where  vr  is  the  measurement  variance  (along  the  direction  of  the  measurement)  and 


C  =  $ 


vr  0 
0  10  *  Vr 


<f> 


cos  9  —  sin  6 
sin  9  cos  9 


Monte  Carlo  Particle  Filter:  In  comparison  to  the  EKF,  the  authors  construct  a  Monte 
Carlo  particle  filter.  Following  similar  “prediction”  and  “update”  steps  as  the  EKF,  the 
authors  start  with  Np  particles  and  compute  xpik)  for  each  p  using  the  process  model  given 
above.  Each  particle,  p,  is  assigned  a  weight,  w(p)  such  that  w(p)  =  p(rp[m(k)),  where  rp 
is  the  distance  between  Xb{k )  and  the  projected  position.  After  computing  weights  for  all 
of  the  points,  the  weights  are  rescaled  such  that  the  sum  of  the  weights  for  all  points  equals 
1.  Finally,  for  each  p,  xp ( k  +  1)  is  randomly  chosen  from  the  prescribed  collection. 

Comparison:  The  authors  found  that  using  identical  beacon  locations,  beacon  returns, 
robot  trajectories  and  noise  covariance  matrices,  the  average  estimation  error  from  the 
Kalman  filter  simulation  is  slightly  better  than  the  Monte  Carlo  simulation  (0.73  feet  versus 
0.93  feet),  and  the  Kalman  filter  only  requires  0(Nb )  computations  while  the  Monte  Carlo 
simulation  requires  0(NbNp )  computations.  The  authors  conclude  that  this  is  a  signifi¬ 
cant  difference  in  computation  cost  because  the  smallest  Np  that  gives  reasonable  results  is 
approximately  200. 

Simultaneous  Localization  and  Mapping  with  EKF:  The  authors  extend  the  EKF  de- 


147 


APPENDIX  A.  SINGLE-BEACON  NAVIGATION 


scribed  above  to  the  situation  where  beacon  locations  are  approximately  but  not  exactly 
known.  The  state  vector  consists  of  the  vehicle  position  as  well  as  the  beacon  locations  and 
the  process  model  is  updated  accordingly  assuming  the  beacon  locations  do  not  move.  The 
authors  assume  that  range  measurements  are  transformed  into  a  position  measurement  with 
zero-mean  Gaussian  noise  as  described  above. 

In  simulation,  the  range  measurements  noise  variance  is  v  —  1  and  the  variance  of 
the  initial  robot  and  beacon  estimates  is  set  to  25  (i.e.  an  expected  error  of  5  feet).  In  the 
simulation  shown  in  the  paper,  the  average  error  improved  from  5.13  feet  initially  to  0.77 
feet  after  travelling  approximately  60  feet. 
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Linear  Kalman  Filter  Derivation 


The  following  is  a  brief  derivation  of  the  prediction  and  update  equations  for  the  Kalman 
filter.  We  assume  that  we  have  a  discrete-time  process  model  and  observation  model  of  the 
form 


xk+1  =  Fxk  +  Buk  +  wk 
zk  =  Hxk  +  vk 


(B.l) 

(B.2) 


where  x  is  the  state  vector,  uk  is  the  input,  zk  is  the  measurement,  and  w  ~  J\f( 0,  Q  )  and 
v /,  ~  AT(0,  R)  are  independent  zero-mean  Gaussian  noise. 
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B.l  Assumptions 

In  deriving  the  Kalman  filter  we  seek  a  recursive  unbiased  estimator  for  the  plant  state 


M fc+i|fc  —  E[xk+ 1] 

(B.3) 

=  E[xk) 

(B.4) 

such  that  the  sum  of  the  variance  of  the  state  estimate 

E[(xk- nk\kY {xk~  Hk\k)\  (B-5) 


is  minimized. 

B.2  Process  Prediction 

Using  the  unbiased  estimator  assumption  (B.3)  we  compute  the  prediction  equation  for 
fx,  the  mean  of  x,  as  follows, 

Mfc+i|fc  =  E[xk+1]  (B.6) 

=  E[Fxk  +  Buk  +  wk] 

=  FE[xk }  +  Buk  +  F^wk\r° 

Mfc+i|fc  =  F^k\k  +  Buk  (B.7) 
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where  we  rely  on  the  fact  that  xk  and  wk  are  uncorrelated.  Using  (B.7)  we  can  solve  for 
the  prediction  equation  for  X,  the  covariance  of  x,  as  follows, 

Sfc+ijfc  =  cov(xk+ 1  -  nk+1]k)  (B.8) 

=  cov(Fxk^Bn^^ 

=  cov(F(xk  -  nk\k)  ~  Wk) 

=  cov(F(xk  -  nk]k)  +  cov(wk ) 

=  Fcov((xk  -  k))FT  +  cov(wk ) 

=  FTik\kFT  +  Q  (B.9) 

where  we  use  the  fact  that  F(fik\k  —  xk)  and  wk  are  uncorrelated. 

B.3  Measurement  Update 

We  assume  when  deriving  the  measurement  update  equations  that  the  update  is  a  linear 
combination  of  the  previous  state  estimate  and  the  measurement: 

Vk\ k  =  AVk\k-i  +  Kzk  (B.  10) 
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where  both  A  and  K  are  unknown.  Using  equation  B.4  to  solve  for  A: 

0  =  E[xk  -  (B-U) 

=  E[xk  -  Ank |fc_x  -  Kzk\ 

=  E[xk  -  A/xk |fc_x  -  K(Hxk  +  ufc)] 

=  E[xk  -  KHxk]  EiAfx^} 

0  =  (I  ~  KH)E[xk ]  -  Ank |fc_! 

A  —  I  —  KH  (B.12) 

Substituting  (B.12)  into  equation  (B.10)  we  arrive  at  the  measurement  update  equation  for 
the  mean  of  x  in  terms  of  K : 

=  Mfc|fc-i  +  K(zk  -  H\ik |fc_r)  (B.13) 
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We  compute  the  measurement  update  equation  for  the  covariance  of  x  in  terms  of  K 

£/#  =  cov(xk  -  nklk)  (B.14) 

=  cov(xk  -  /ifc|fc_!  +  KHnk |fc_r  -  Kzk) 

=  cov(xk  -  /ifc|fc_j  +  KHnk\k_i  ~  KHxk  -  Kvk ) 

=  cov((I  -  KH)(xk  -  -  ■K'Ufc) 

=  cou((J  -  KH)(xk  -  /%.|fc_i))  +  cov(Kvk ) 

=  (/  -  KiT)cov(xfc  -  -  -ft:#)7  +  Kcov(r;fc)KT 

=  (/  -  KiT)Sfc|fe_1(J  -  KiT)T  +  KRK\  (B.15) 

B.4  Kalman  Gain 

To  solve  for  the  Kalman  gain,  K ,  we  use  the  fact  that  we  are  seeking  a  recursive  estima¬ 
tor  that  minimizes  the  sum  of  the  variance  of  the  state  estimate,  and  we  note  that  minimizing 
the  sum  of  the  variance  is  identical  to  minimize  the  trace  of  the  covariance  matrix, 

Ei(xk  -  Mfc| kV(xk  -  Mfciifc)]  =  trE[(xk  -  Hk]k){xk  -  Mfc|fe)T]  (B .16) 

=  trTik\k.  (B.17) 

Substituting  the  measurement  update  equation  (B.15)  for  the  covariance,  we  minimize 
the  trace  by  setting  the  partial  derivative  equal  to  zero  and  note  that  the  trace  of  the  partial 
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derivative  equals  the  partial  derivative  of  the  trace: 

d  d 

°  =  dK^trT,k  =  trdK^k  |fc^' 

Substituting  the  definition  of  £fc|fc  from  (B.  15)  into  (B.18)  we  obtain 

0  =  trM{^  -  KH ~  /v  //  +  krkt} 

—  tr  —  KHHk  |fc_!  —  Sfc|  k-\HT  Kt 

+  Kt  +  KRKt} 

=  tr{ JJT  -  if  T  +  JTt 

+  KiTSfcifc.xiT7  +  /\  R  +  KRt}. 

Because  ^k\k-i  and  R  are  symmetric  by  definition  we  can  further  simplify  (B.19) 

0  =  tr{ — 2Sfc|fc_i  +  ‘2KHY!ik\k—iH^  +  2  KR\. 

The  constraint  (B.20)  is  satisfied  by  setting  the  argument  of  the  trace  equal  to  zero 

0  =  — 2Sfc|k__i_tf T  +  2K  _t/Sfc|fc-i-H"T  +  2  KR 

which  allows  us  to  solve  for  the  Kalman  gain 

K  =  Efc,fc_1ifT(JfE*|*_1ifT  +  R)-1. 


(B.18) 


(B.19) 


(B.20) 


(B.21) 


(B.22) 
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Linear  Information  Filter  Derivation 


The  following  is  a  brief  derivation  of  the  linear  information  filter  in  the  context  of  a 
linear,  one  degree-of-freedom  plant  model.  The  infoimation  filter  is  often  referred  to  as 
the  dual  of  the  Kalman  filter.  The  Kalman  filter  recursively  estimates  the  mean,  fi,  and 
covariance,  X,  of  the  random  variable  x,  where 


/r  =  E[x\ 

(C.1) 

h 

1 

•Jt 

l 

it 

W 

(C.2) 

The  information  filter  is  based  on  the  explicit  normalization  of  the  random  variable  x  by 
its  covariance  S  such  that  the  information  filter  recursively  estimates  the  mean,  77,  and 
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covariance,  A,  of  the  normalized  random  variable  £  lx 


77  =  E[E~1x\  =  (C.3) 

A  =  E  -  XrVXXT1*  -  S^V)T]  =  5T1  (C.4) 

where  77  is  referred  to  as  the  information  vector,  and  A  the  information  matrix  [8, 70]. 


C.l  Kalman  Filter  Review 

The  derivation  of  the  information  filter  is  presented  in  the  context  of  the  Kalman  filter. 
Thus  we  start  with  a  brief  overview  of  the  Kalman  filter.  For  this  derivation  we  will  assume 
that  B  =  0. 

The  Kalman  filter  prediction  equations  from  Appendix  B  are 


Mfc+llfc  -  F kVk\k  T 0 

(C.5) 

^k-\-l\k  F* k^k\kF ~\~  Qk’ 

(C.6) 

The  Kalman  filter  measurement  update  equations  from  Appendix  B  are 


A0c|fc  —  +  K(zk  —  H/if e|fe— r)  (C.7) 

^ k\k  =  {I  ~  KkHk)Tik |fc_i  (C.8) 

Kk  = 'Ek\k~iHj  (Hk'Ek\k_iHj  +  Rk)  1  (C.9) 


156 


APPENDIX  C.  LINEAR  INFORMATION  FILTER  DERIVATION 

where  (C.8)  is  derived  from  the  form  of  given  in  (B.15)  as  follows 


—  (I  —  K  kH  k)Y,k\k_i(I  —  KkHk)T  +  KkRkKk 


(C.10) 


—  S fc|fc— !  —  KkHkT,k  |fc_i  —  Sfc|  k^iHjKj  +  K  kH  k’Etyk-iHj  Kk  +  KkRkK\ 


—  —  KkHkY,k  |fc_!  —  Sfc|  k-iH'kKl  +  K  k(H  kY,k\k_iHj.  +  Rk)K 


-T 


^k\k—l  ^k\k—  1  Hk(HkT,k\k_iHk  +  Rk)  HkT,k\k_ 
^-ik\k—iHk  (H k^ k\k—\H k  T  R'k)  Hk^jk |fc_i 


+  -Rfc)  1i3"fcEfc|fc_1 

1  1  H~[  (HkY,k\k_iHj  +  i?fc)  1iT/cI]fc|fc_1 


(J  -K  kH  k)^ i 


noting  that  (iTfcSfc|fc_iiTj  +  i?fc)  1  is  symmetric. 


157 


APPENDIX  C.  LINEAR  INFORMATION  FILTER  DERIVATION 


C.2  Plant  Process  Model 


We  assume  a  1  degree-of-freedom  constant-velocity  linear  process  model  for  the  plant 


x  = 


(C.ll) 


where  the  state  vector  x  is  position  and  velocity  along  the  y-axis 


x  = 


y 

y 


(C.12) 


and  w  is  independent  zero-mean  Gaussian  process  noise  in  the  acceleration  term. 


w  ~  A/”(0,  q) 


(C.13) 


The  process  model  is  discretized  according  to  standard  methods  [8],  resulting  in  the  discrete¬ 
time  linear  process  model 


Xk- i-i  F'kXk  T  Wk 


(C.14) 


Fk 


=  e 


FT 


1  T 
0  1 


(C.15) 
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where  F  is  defined  in  (C.  11),  T  is  the  discrete  time  step  and 


xk  = 


Vk 

Vk 


(C.16) 


is  the  discrete  state  vector.  The  discretized  process  noise,  wk  is  zero-mean  Gaussian 


wk  ~  A/”(0,  Q) 


(C.17) 

(C.18) 


where  the  covariance  matrix  is 


Q 


1 

3 

lrp2 

21 


Q- 


(C.19) 


Note  that  even  though  Fk  and  wk  are  shown  with  a  subscript  denoting  the  time  step  k,  they 
are  in  fact  constant  and  do  not  depend  on  time. 
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C.3  Process  Prediction 


To  derive  the  prediction  equations  for  the  information  matrix,  we  start  with  the  defini¬ 
tion  of  A, 


Afc|fc-1  ^ k\k-l 

—  +Qk-i)  1 


l-l 


=  (^k-i  +  Qk~i)  1 

Afc|fc_i  =  Ak_i  —  Ak_i  (Ak_i  +  Qk\)  1Ak_ 


(C.20) 


(C.21) 


where  we  define  Ak\  such  that  Ak_x  =  Fj_  Sfc^1|fc_1  Fk\  and  we  invoke  the  matrix 
inversion  lemma  from  Bar-Shalom  [8,  p.  23] 


(A  +  BCBr)~1  =  A”1  -  A~1B(BtA~1B  +  C~1)-1BtA~1.  (C.22) 
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Similarly  for  77, 


“Hklk-l  =  ^k\k-l^k\k-l 

=  [J71  k-i^k-i\k-iF  k- 1  +  Qk- 1]  F  k-iHk-i\k-i 

=  Fk_i('Ek_i\k_i  +  Fk\Qk_1F^_1)F^_1  Fk_1nk_1\k_1 

F  k\Q  k-iF~k_i)  1  ^hff^F^k- i|fe— 1 

■v 


—  Fk_  1  (£*_!,*_! 


=  F 


.T-1 

fc-1 


V 

matrix  inversion  lemma 

i  —  l  _  yi  —  1 

Jk-l\k-l  ~  ^k-llk-l 


F-k\ 


■^fc—  1  ^fc— pfc— 1  Mfc— llfc—  1 


jt»T  1  ttiT  1  — 1  |7»— 1  (  pT  1  •Vi  — 1  771— 1 

^  fc-1  ^  fc-1  ^fc_i|fc_i-r  fc-ll^  fc-1  ^jfe-llfc-l-^  fc-1 

V  J  V 


(C.23) 


-1 


fA  +QA)_I  f  A 


-A  fc-i 


V* 

A-k-i 


'  ^fc— l|fc— lMfe— 1|A— 1 

" - V - ' 

^fc-llfc-l 


^fclfc-l 


—  [I  —  Ak-i(Ak-i  +  Qk\)  1]-F’fe-1»7fe- i|fc— 1- 


(C.24) 


C.4  Process  Prediction  with  Augmentation 

In  this  example,  the  system  state  is  augmented  with  the  newly  predicted  plant  state  for 
every  prediction,  so  that  all  historic  states  are  retained  and  the  system  state  vector  grows 
over  time.  To  avoid  ambiguity,  we  will  use  xt  to  denote  the  system  state,  which  is  the  entire 
state  of  the  system  including  current  and  historic  states.  The  system  state  consists  of  the 
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current  plant  state  xk  and  a  collection  of  historic  plant  states  xp 


xt  = 


Xk 

Xp 


(C.25) 


For  this  derivation  we  will  assume  that  xp  consists  of  a  single  historic  plant  state.  During 
each  prediction,  the  process  model  for  the  system  augments  the  system  state  with  a  new 
plant  state,  xk+1.  Thus  the  system  state  vector  grows  at  each  time  step  to  include  not  only 
the  collection  of  historic  states  and  the  current  plant  state  xk,  but  also  the  prediction  of  the 
next  plant  state  xk+i  as  governed  by  the  plant  process  model  in  equation  C.14.  We  will 
drop  the  subscript  k  on  F  and  w  for  clarity  of  notation  for  the  remainder  of  the  derivation. 
We  can  now  write  the  process  model  for  the  system  as 


F  0  ' 

"  I  " 

Xi+l  = 

I  0 

Xi  + 

0 

0  I 

0 

w 


Gt 


(C.26) 


where 


xt+i 


X  /,■ .  i 

Xk 


(C.27) 


xp 

We  define  the  mean  and  covariance  of  the  system  state  estimate  before  prediction  as 


/V 
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which,  in  the  information  form,  are 


At  =  S'1  = 


It  =  = 


A  pk  A 


pp 


A-kk^k  H-  A-kpl-Lp 

Vk 

A-pk/J'k  A-pp/J'p 

.  % . 

The  mean  and  covariance  of  the  system  state  estimate  after  prediction  are 


(C.30) 

(C.31) 


Mt+i  =  FtFt 

F  0 
I  0 
0  I 
F^k 
Vk 
Fp 

St+i  =  FtHtFj 


F 

I 

0 


0 

0 

I 


vp 


GtQG 

^-‘kk 

^ pk 


iT 


F'EkkF 

Si .kFT 


Q 


'F‘PkFT 


T 

t 

kp 

y 

^pp 

FYikk 

kk 
^ pk 


F 
0 

FT,kp 

^ kp 

v 

pp 


I 

0 


0 

I 


i 

"  i " 

+ 

0 

- 

0 

Q 


(C.32) 


(C.33) 


(C.34) 


TOO 


(C.35) 
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which,  in  the  information  form  [25],  are 


Ai+i  —  S 


-l 
t+ 1 


F'F>kkFT  +  Q 

FT,kk 

FT,kp 

S kkFT 

^kk 

^kp 

'£pkFT 

^ pk 

v 

^pp 

Q  1 

-FQ  1 
0 


-Q~lF 

FQ  1F  +  A  kk 

A-pk 


0 

A-kp 

A. 


pp  j 


rlt+ i  ~ 


Q  1 

1 

«e> 

"■q 

0 

i - 

a. 

FQ1 

FtQ1F  +  Akk 

A-kp 

Mfc 

0 

A-pk 

i 

& 

.  . 

0 

^-kkl-^k  T  ^-kp/J'p 
A-pkH-k  T  A-ppfip 


0 

Vk 

.  v p . 


(C.36) 

(C.37) 


(C.38) 


for  a  linear  plant,  where,  as  noted  in  [26],  prediction  with  augmentation  results  in  (C.36) 
having  a  sparse  tridiagonal  structure.  As  noted  in  [25],  the  derivation  of  (C.36)  through  the 
inversion  of  Ef+i  is  tedious,  but  the  result  can  easily  be  validated  by  matrix  multiplication 
to  show  that  At+]  St+1  =  I.  Note  that  (C.36)  can  be  written  as  the  sum  of  a  two  matrices 
consisting  of  a  constant  matrix  plus  the  previous  information  matrix 


Q  1 

-Q'F 

0  " 

"  0 

0 

0 

-FQ1 

F  Q  lF 

0 

+ 

0 

A-kk 

A-kp 

0 

0 

0 

0 

A-pk 

A.pp 

(C.39) 
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C.5  Measurement  Update 

The  measurement  update  equation  for  the  information  matrix  is  straightforward  to  de¬ 
rive  using  the  matrix  inversion  lemma, 


-l 


i  -i 


^■k\k  ^k\k 

=  [(/  —  K  kH  k)^k\k-i\ 

[^fe|fe— 1  ^k\k—  1  Hj(Hk'Ek\k-iHk  +  Rk)  1Hk'S,k\k-i] 

=  ^k\k-i  +  HTkRk1Hk 


(C.40) 


-i 


—  Afc|fc_!  +  HjRk  lH 


(C.41) 


where  Zk  is  the  measurement,  II k  is  the  linear  measurement  matrix,  and  Rk  is  the  covariance 
matrix  of  the  measurement  noise. 

To  derive  the  measurement  update  equation  for  the  information  vector,  we  will  use  the 
fact  that  Kk  =  'Ek\kHjRk1, 


Kk  =  ’Sk\k-iH~^(Hk'^k\k-iH~l  +  Rk)  1  (C.42) 

K  k(H  k^k\k-iH~k  +  Rk)  — 

KkRk  —  (I  —  K  kH  k)^k\k-iHj 
Kk  —  (I  —  KkH^k^HjR-1 
Kk  =  (Xfc|fc_i  —  K kH k^k\k-i)^^^i^k\k-iHj.  Rk  1 
Kk  =  T,k\kH~lRk  1. 
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The  derivation  for  the  information  vector  is  then 


'Hk] k  ~  (C.44) 

=  \^k\k-l  +  Kk{.zk  ~ 

=  '^‘k\ktJjk\k-l  ~  ^k\k^ kH k^k\k-l  +  '^lk\k^kZk 

=  (Afc|fc_i  +  Hk  Rk  Hk)nk\k_i  ~~  ^%\k^k\kHk  Rf.  Hk[ik\k_l  +  Rk  zk 


—  ^k\k-i^k\k-i  +  Hk  Rk  Hkl^k\k-i  HkRk  Hkfik^k_1  +  HkRk  zk 
*7fc|fc  =  »?fc|fc-i  +  Hl Rklzk-  (C.45) 


C.6  Marginalization 


The  derivation  for  the  generalized  equations  for  marginalization  are  adapted  from  [25]. 
This  derivation  uses  the  result  for  the  inverse  of  a  nonsingular  block  2x2  matrix  from 
Bar-Shalom  [8,  p.21],  which  is  repeated  here  for  convenience. 

The  inverse  of  a  nonsingular  block  2  x  2  is 


1 

53 

to 

_ 1 

1 

1 

I*: 

Vl2  ’ 

1 

1 

0? 

_  v21 

^22  _ 

(C.46) 
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where 


v,,  =  Py L1  +  Pu]Pv2V22P2lPul  =  (Pn  -  P12P221P2i)~1  (C.47) 

hi  2  =  -PyPviV-n  =  ~VuPnP22  (C.48) 

V21  =  -V22P2\Py  =  -P22]  P2\VU  (C.49) 

v22  =  p-}  +  Py2l P2lVuPv2Py2l  =  {P22  -  P21P11P12)-1.  (C.50) 


Replacing  P  with  A  and  V  with  £ 


1 

e 

<1 

-1 

y 

^ aa 

1 

7a 

s 

W 

A- /3  a 

A/3/3  . 

3a 

S/9/3  . 

(C.51) 


we  solve  for  the  elements  of  £  because  in  the  covariance  form  if 


X(a,/3) 


^ aa  ^ a/3 


(C.52) 


then 


X(a)  =  XQQ 


(C.53) 


and  thus 


A(a)  =  S(cr)-1  =  £^ 


(C.54) 
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where  a  are  the  states  to  be  kept  and  /3  are  the  states  to  be  marginalized  out.  Thus  marginal¬ 
izing  out  /3  results  in 


r){a)  =r)a-  AapApprjp  (C.55) 

A(a)  =  Aaa  -  Aa/3AppApa.  (C.56) 
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Linear  Information  Filter  Example 


Presented  here  is  an  example  implementation  of  the  linear  information  filter  in  the  con¬ 
text  of  a  strictly  linear,  one-degree-of-freedom  system.  The  linear  information  filter  equa¬ 
tions  are  used  here  without  derivation.  Readers  are  referred  to  Appendix  C  for  the  linear 
information  filter  derivation. 

This  example  consists  of  a  two-node  system,  the  nodes  labelled  ship  and  vehicle  respec¬ 
tively.  Each  nodes  has  one  degree  of  freedom,  moving  along  the  y-axis.  Both  the  ship  and 
the  vehicle  have  a  linear,  constant-velocity  process  model.  Three  types  of  measurements 
are  possible,  position  measurements  for  the  ship,  velocity  measurements  for  the  vehicle, 
and  range  measurements  between  the  ship  and  the  vehicle. 

The  goal  of  this  example  is  to  explore  the  differences  between  a  centralized  filter  that 
has  access  to  all  sensor  measurements  from  both  the  ship  and  the  vehicle  in  real-time  and 
a  decentralized  vehicle-based  filter  that  only  has  access  to  vehicle  sensor  measurements  in 
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real  time.  In  the  decentralized  filter  the  vehicle  receives  asynchronous  data  transmissions 
from  the  ship  concurrent  with  range  measurements  from  the  ship,  but  does  not  have  access 
to  ship  measurements  of  the  ship  state  estimate  in  real  time. 


D.l  Process  Model 

We  assume  a  constant-velocity  process  model  for  both  the  vehicle  and  the  ship 


xv 


X 


S 


(D.l) 


(D.2) 


where  the  state  vectors  xv  and  xs  contain  the  position  and  velocity  along  the  y-axis  of  the 
vehicle  and  the  ship  respectively 


xv 


xs 


Vv 

Vv 

Vs 

Vs 


(D.3) 

(D.4) 
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and  wv  and  ws  are  independent  zero-mean  Gaussian  process  noise  in  the  respective  accel¬ 
eration  terms 


(D.5) 

Af(0,qs). 

(D.6) 

The  process  models  are  discretized  according  to  standard  methods  [8].  The  resulting 
discrete-time  linear  process  models  are 


+  l  ^ vk*^vk  ~E  ^vk 


F  =  eFvT  = 

-*■  vk  c 


1  T 
0  1 


Xsk+i  =  FSkxSk  +  wSk 


Fsk  =  eF‘T  = 


1  T 
0  1 


(D.7) 

(D.8) 

(D.9) 

(D.10) 


where  Fv  and  Fs  are  defined  in  (D.l)  and  (D.2)  respectively  and  T  is  the  discrete  time 
step.  The  discretized  process  noise,  wVk  and  wSk,  are  zero-mean  Gaussian 


wVk  ~A/"(0 ,QV)  (D.ll) 

wSk~AT(0,Qs)  (D.12) 
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where  the  covariance  matrices  are 


2 

lrp2 


(D.13) 


2 


(D.14) 


Note  that  even  though  FVk,  FSk,  wVk,  and  wSk  are  shown  with  a  subscript  denoting  the 
time  step  k,  they  are  in  fact  constant  and  do  not  depend  on  time. 


D.2  Process  Prediction  and  Augmentation 


The  ship  and  vehicle  process  models  have  identical  process  prediction  and  augmen¬ 
tation  equations.  For  simplicity  of  notation  I  will  derive  a  general  form  of  the  equations 
without  either  the  s  or  v  subscripts.  This  system  is  formulated  such  that  the  complete  state 
vector  consists  of  both  current  and  historic  states.  For  every  prediction,  the  current  state  is 
augmented  with  the  newly  predicted  state,  thus  all  historic  states  are  retained  and  the  state 
vector  grows  over  time.  To  avoid  ambiguity,  I  will  use  xk  to  denote  current  plant  states,  xp 
to  denote  historic  plant  states,  and  a  different  font  xt  to  denote  the  system  state,  which  is 
the  entire  state  of  the  system  including  both  current  and  historic  plant  states 


(D.15) 


xfc  = 


X 


V 


Consider  a  system  in  which  the  plant  state  is  governed  by  the  following  constant- 
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velocity,  discrete-time,  linear  process  model  similar  to  (D.7)  and  (D.9) 


■t'fc+i 


1  T 

0  1 

— v' — 

f 


xk  +  w 


xk  = 


Vk 

Vk 


(D.16) 


(D.17) 


where  w  ~  7V(0,  Q ),  Q 


lrp3 

3 

1t-i2 

2 


q  and  T  is  the  discrete  time  step. 


The  process  model  for  the  system  augments  the  system  state  with  a  new  plant  state, 
xk+i,  which  is  the  prediction  forward  of  the  current  plant  state 


F  0 

I 

Xfc+l  — 

I  0 

Xfc  + 

0 

_  0  I 

0 

Fk 


W 


(D.18) 


where 


xfc+i 


fc+ i 

xk 


Xp 

The  mean  and  covariance  of  the  system  state  estimate  before  prediction  are 


(D.19) 


Vk 

VP 


Vk\ k  —  ^[xfc]  ~~ 

L  v P  j 

=  E[(xk  —  fik |fc)(xfc  —  fik |fc)  ]  = 


Eikk 

Ejpk 


Eikp 

X 


pp 


(D.20) 

(D.21) 
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which,  in  the  information  form,  are 


A-k\k 

Vk\k 


A-kk 
A-pk 

^k\k^k\k  = 


A-kp 

A-pp 

^-kk/J'k  ~~f  ^-kpt^p 
A-pkf^k  "f"  ^-ppL^p 


(D.22) 

(D.23) 


The  mean  and  covariance  of  the  system  state  estimate  after  prediction  are 


Mfc+ilfc 


Jk-\-l\k 


Fvk 

F’EkkF 

Sfcfc-F 

Jpk 


+  Q  FUkk 
T  ’F'kk 

^ pk 


F'Ekp 

Flkp 

XL 


jpp 


which,  in  the  information  form  [25],  are 


A 


k+l\k 


=  £ 


-l 


k-\-l\k 

Q1 

~FQ  1 
0 


rlk+l\k  -^-A;+l|fcMfe+l|fc 


-Q~lF 

FTQ~1F  +  Akk 

Apk 

o  1 


Vt 


0 


A 

A 


kp 


pp 


(D.24) 


(D.25) 


(D.26) 

(D.27) 


Note  that  (D.26)  can  be  written  as  the  sum  of  a  two  matrices  consisting  of  a  constant  matrix 
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plus  the  previous  information  matrix 


Q  1 

-Q-'F 

0  ' 

"  0 

0 

0 

Afc+i|jfc  — 

-FQ1 

FtQ~1F 

0 

+ 

0 

A-kk 

A-kp 

0 

0 

0 

0 

A-pk 

i 

& 

D.3  Marginalization 

In  this  example,  states  that  do  not  have  range  measurements  associated  with  them 
are  marginalized  out  after  a  given  time.  For  completeness,  the  generalized  marginaliza¬ 
tion  equations  are  shown  here  for  two  scenarios.  The  first  scenario  is  when  the  one  or 
more  states  at  the  bottom  of  the  system  information  vector  (i.e.  the  oldest  states)  are  to 
be  marginalized  out.  In  this  case  the  information  vector  can  be  split  into  two  partitions, 
one  associated  with  the  states  to  keep,  rja ,  and  the  other  associated  with  the  states  to  be 
marginalized  out,  rj3 


ri(a,f3) 

A(a,f3) 


la 


ip  \ 

A-olol 

Aa/3 

■k-(3a 

AA3 

Marginalizing  out  [3  results  in 


(D.29) 

(D.30) 


lipt)  =  la~  AapA^T]p 


(D.31) 
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Note  that  77(a)  ^  rja  after  marginalization. 

In  the  second  scenario,  one  or  more  states  in  the  middle  of  the  state  vector  are  to  be 
marginalized  out.  In  this  scenario  the  information  vector  is  split  into  three  partitions,  where 
the  states  associated  with  r)fi  are  again  to  be  marginalized  out. 


*7a 

77(03  Pi  7)  = 

*7/3 

:  777  - 

A-a/3  A-ot'y 

A(a,  /?,  7)  = 

A-/3a 

A/3/3  A^7 

A-rya 

A7^3  "^^"77 

Marginalizing  out  (3  results  in 


(D.33) 


(D.34) 


»7(a3  7) 
A(a,7) 


*7« 

*77 


A, 

A 


a/3 

7/3 


A 


Q7 


^V/V/T, 


l-/ya 


'77 


A/3j*7/3 


A 

A 


a/3 

7/3 


A/3/3 


A/3a  A/37 


(D.35) 

(D.36) 


Note  that  the  second  scenario,  and  indeed  any  general  case  concerning  the  partitioning 
of  77  and  A,  reduces  to  the  two-partition  scenario,  because  77  and  A  can  be  reordered  into 
two  partitions  using  an  appropriate  orthonormal  permutation  matrix.  See  [25]  for  more 
details. 


176 


APPENDIX  D.  LINEAR  INFORMATION  FILTER  EXAMPLE 


D.4  Measurement  Update 

Given  a  generalized  observation  model  for  a  scalar  measurement 


zk  =  Hkxk  +  vk  (D.37) 

where  zk  is  the  measurement,  Hk  is  the  linear  measurement  matrix,  and  vk  ~  A/”(0,  rk)  is 
the  measurement  noise,  the  measurement  update  equations  for  the  information  filter  are 

lk\k  =  Vk\k-i  +  HkrklZk  (D.38) 

=  Afc|fc_!  +  Hjrk  1Hk.  (D.39) 

D.5  Observation  Models 

The  full  state  vector  for  this  system  contains  current  and  historic  states  for  both  the 
vehicle  and  the  ship.  However,  for  simplicity  of  notation,  the  observation  models  below 
are  written  in  terms  of  a  simplified  state  vector  consisting  of  only  the  current  vehicle  and 
current  ship  states  except  where  otherwise  noted. 
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D.5.1  Ship  GPS  Measurement 


The  position  measurement  for  the  ship  mimics  a  GPS  measurement.  The  position  mea¬ 
surement  equation  is 

Z-gps  Vs  +  Vgps  (D.40) 

where  vgps  ~  A/"(0,  rgps).  We  can  rewrite  (DAO)  in  matrix  notation  as 


Zgps  ~ 


0  0  10 


1 

i 

a 

i _ 

y 

i 

<*> 

l _ 

T  Vgps- 


(D.41) 


Ha 


D.5.2  Vehicle  Velocity  Measurement 

The  velocity  measurement  for  the  vehicle  mimics  a  Doppler  velocity  log  measurement. 
The  velocity  measurement  equation  is 


vel  Vv  T  Vvei 


(D.42) 


where  vvei  ~  N{ 0,  rvei).  We  can  rewrite  (D.42)  in  matrix  notation  as 


%vel 


0  10  0 

1 

l _ 

^ - Sr* - r 

✓ 

xs 

Vvel' 


(D.43) 


h„ 
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D.5.3  Range  Measurement 

The  range  measurement  between  the  vehicle  and  ship  in  this  example  is  chosen  to 
mimic  a  range  measurement  made  underwater  using  the  time-of-flight  of  an  acoustic  signal, 
except  that  the  measurement  here  is  linear.  The  range  measurements  in  this  example  are 
always  made  from  the  ship  to  the  vehicle.  Because  the  travel  time  of  an  acoustic  signal 
underwater  is  non-negligible,  the  range  measurement  is  made  between  the  receiver  (the 
vehicle)  at  the  current  time  step  and  the  sender  (the  ship)  at  an  earlier  time  step. 

The  range  measurement  equation  between  the  ship  and  the  vehicle  in  this  problem  is 


z 


mg 


Uvk  Vsk_  1  +  Vrng 


(D.44) 


where  4  —  tk- 1  is  the  time-of-fight  of  the  acoustic  transmission  and  the  noise  vrng  ~ 
A/”(0,  rrng)  represents  the  imprecision  in  the  distance  measurement.  Note  that  the  range 
measurement  is  a  signed  number  in  this  model  in  order  to  disambiguate  which  side  of  the 
ship  the  vehicle  is  on.  We  can  rewrite  (D.44)  in  matrix  notation  as 


^ mg 


10-10 


1 

l 

8 

1 _ 

y 

Xsk- 1 

+  v, 


mg- 


Hr 


(D.45) 
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D.6  Centralized  Implementation 


In  the  centralized  implementation,  the  filter  estimates  the  current  and  historic  states 
of  both  the  vehicle  and  the  ship.  We  assume  that  the  filter  has  access  to  all  vehicle  and 
ship  sensor  measurements  in  real  time.  The  ship  and  the  vehicle  start  at  t0  and  the  filter  is 
initialized  to 


^oio 

Aq|o 


Vvo 


Vso  _ 

A-VqVo  ® 

0  -A-SOSO 


(D.46) 

(D.47) 


Figure  D.l  shows  the  example  system  used  here  to  compare  the  centralized  and  the 


decentralized  implementations  of  the  information  filter.  In  addition  to  the  measurements 


shown,  the  vehicle  state  at  f0  and  the  ship  state  at  t\  are  marginalized  out  after  the  t\  to  t2 


prediction  with  augmentation  step. 


Figure  D.l:  Vehicle  and  ship  trajectories  with  sensor  measurements. 
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Prediction  with  Augmentation  t0  — >  t  , 

A  prediction  with  augmentation  step  transitions  the  state  from  t0  to  t\\ 
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1  fJa;1-?.. 
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1  s> 

G* 

l 

0 
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Q 71 

0 

0 

~ F]Q 71  Fj 

(D.48) 


"  0 

0 

0 

0 

+ 

0 

A-VOVO 

0 

0 

0 

0 

0 

0 

.  0 

0 

0 

^■SOSO  - 

0 

0 

-QslFs 
Q^FS  +  A 


SosO  . 


(D.49) 
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Range  Measurement  at  tx 

Incorporating  the  range  measurement  zrng(ti)  gives  us 


rh\Zrng1  ~  Vl\0  +  H rngT  rngZrng(t  \  , 

1 

0 


Vvo 

0 

Vso  J 


0 

Vv0 

0 

Vso  J 
z, 


r  l 
Vvo 
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0 

0 

0 

-1 

0 

zri 


0 

0 

— zr 


Vso 


~T  1  _ 


Z. 


rng  (J~  1 


rng 


(D.50) 


(D.51) 


where  we  define  zri 


Zrng  (^1 ) 
P  rng 
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AlU 

±\Z' 


mg ^ 
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0 

0 

V  mg 

0 

0 

0 

0 

0 

0 

Q71 


-1  0 


—R, 
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(D.52) 


SQSQ 


+  Rr 

(D.53) 


where  we  define  Rr 


0 

V  mg 

0  0 
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Ship  GPS  Measurement  at  G 

Next  we  incorporate  the  ship  GPS  measurement  zgps(ti). 


Vi[, 


'rngi  i^gps-^ 


Vl~\zrrLg-^  ' 
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Vv0 
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Vso  zr\ 
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Vvo 
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+ 


0 

0 
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Z, 


91 


Vso 


1 


(D.54) 


(D.55) 


where  we  define  zgi 


1 

0 


zgpsV  l) 

rgPs 
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^  1  | Zrng  ]  i%gps  ]  ^  1  | Zmg  |  T  F gps^“ gps^ 9PS 
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(D.56) 


R, 

0 

-Qs'Fs 

fJ  Qs1fs  A  ASqSo  A  Rr 


(D.57) 


where  we  define  Rg  = 


0  0 
i 

gps 


0 


After  incorporating  all  of  the  measurements  for  this  time  step  we  let 
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mg i  i^gpsi 


(D.58) 

(D.59) 
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Prediction  with  Augmentation  t\  — >  t2 

A  prediction  with  augmentation  step  transitions  the  state  from  t\  to  t%: 
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(D.60) 


Qv 1  -Qv  lFv 
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(D.61) 
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Marginalize  out  xv(to),  Vehicle  State  at  to 

Starting  with  the  information  vector  after  the  prediction  to  t2,  equation  (D.60), 


^?2|l(®«2 )  xv\ >  XV0 ,  XS2  ,  XS1 ,  XSg  ,  ) 


0 

zr  i 

Vvo 


z 


9 1 


Vso  zr\ 


(D.62) 


we  marginalize  out  the  vehicle  state  at  to  using  equations  (D.35)  and  (D.36), 
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(D.63) 


(D.64) 


where 


Vvi  -  Zr\  T  Qy  F'yi^F'y  Qy  Fy  +  \ygyg  )  T)y0 


(D.65) 


187 


APPENDIX  D.  LINEAR  INFORMATION  FILTER  EXAMPLE 


Similarly  for  the  information  matrix, 
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where 


A^lt;i  —  FjQv1Fv  +  Qvx  +  Rr  —  Qv1Fv(FJQv1Fv  +  AV0y0)  1FjQv 


(D.66) 


(D.67) 


(D.68) 


(D.69) 
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Marginalize  out  xs(ti),  Ship  State  at  1 1 

Starting  with  xVo  already  marginalized  out,  equations  (D.64)  and  (D.68),  we  marginalize  out 
the  ship  state  at  t\,  again  using  equations  (D.35)  and  (D.36), 
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(D.70) 


(D.71) 


where  fjvi  was  defined  in  equation  (D.65)  and 


Vs2  ~  Qs  1Fs(FJQs  1Irs  +  Qs  1  +  Rg)  lzgi  (D.72) 

Vso  =  Vs0  -  zr\  +  FjQj1(F]Qj1Fa  +  Q~l  +  Ry)-lzgi.  (D.73) 
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Similarly  for  the  information  matrix, 
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where  AW1U1  was  defined  in  equation  (D.69)  and 
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Vehicle  Velocity  Measurement  at  t,2 

Next  we  incorporate  the  vehicle  velocity  measurement  zvei{t2). 
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where  we  define  zv 2 
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A2| zvei2  -  A2|l  +  HJelrvelHVel 
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(D.82) 


where  we  define  Rv 
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After  completing  the  marginalizations  and  incorporating  all  of  the  measurements  for  this  time 
step  we  let 
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Prediction  with  Augmentation  £2  — 1 ►  £3 

A  prediction  with  augmentation  step  transitions  the  state  from  £2  to  £3 : 
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Range  Measurement  at  f3 

Incorporating  the  range  measurement  zrng(t 3)  gives  us 
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Similarly  for  the  information  matrix, 
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where  we  define  Rr  = 

For  ease  of  notation  let 
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Marginalize  out  xs(to),  Ship  State  at  to 

We  will  now  marginalize  out  the  ship  state  at  to-  This  will  illustrate  what  happens  when  a  state 
associated  with  a  range  measurement  is  marginalized  out,  and  allow  is  to  verify  that  the  solution  is 
reproducible  in  the  decentralized  implementation. 

Using  equations  (D.35)  and  (D.36), 


1 

CO 

s- 

1 _ 

0 

ZV2 

0 

V3\s(XV3  )  XV2  >  XV1 )  *33  )  XS2)  — 

VV! 

- 

R,r 

0 

0 

Vs2  —  Zr  3 

AS2SQ 

-l 

LS0So' 


A  c„0r^T]so 


*T3 

Zv  2 
VV! 
0 

Vs2 


(D.93) 


(D.94) 


where 


Vvi  =  rjv  i  +  RrKoso^o  (D-95) 

Vs2  =  Vs2  -  Zr3  -  AS2S0A soaoVso-  (D-96) 
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Similarly  for  the  information  matrix, 


■^■3|  3  ( 


X 


V3  ?  5  ;  *^53  )  *^S2 


,  X 


X, 


,  x.qn  = 


Qv  1  +  Fr 
-FyQv1 
0 
0 

—Rr 


-Qv  Fv  0 

Fj,  Qv  1Fv  -f  Qv  1  +  Rv  —Qv  1  Fv 


-FvQv1 

0 

0 


0 

0 


0 

—Rr 

0 

0 

0 

0 

T-1 

-^-SOSO 

0 

0 

- 

—Rr 

0 

O 

i 

O 

Qi1 

-Q^Fs 

0 

FjQj1  Fj 

Q S  1FS  +  AS2S2  T  Rr 

A-S2S0 

Qv  1  +  F, 

-QvlFv 

0 

0 

—Rr 

-ftvq-1 

Fy  Qv  1FV  +  Qv  1  +  Rv  - 

-QvlFv 

0 

0 

0 

-FjQv1 

Aviv\ 

0 

-RrVLSO 

0 

0 

0 

Qs 

t 

-Qs'Fs 

0? 

1 

_ 1 

0 

Rr 

lsOvl 

-FjQj1 

A-S2S2 

*-30*2 


(D.97) 


where 


=  Ai,li;i  —  RrAS0S0Rr 

(D.98) 

A-S2S2 

=  Fs  Qs  Fs  +  A s2s2  +  Ftr  —  AS2So AS2S2  ASoS2 

(D.99) 

FrV]SQ 

=  RrAsosoASoS2 

(D.100) 

=  As2s0AsosoRr . 

(D.  101) 

D.7  Decentralized  Implementation 

The  decentralized  implementation  relies  on  two  separate  information  filters,  one  on  the  vehicle 
and  one  on  the  ship.  The  ship  filter  has  no  knowledge  of  the  vehicle  or  the  range  measurements.  The 
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vehicle  filter  receives  data  transmissions  from  the  ship  concurrent  with  range  measurements  from 
the  ship,  but  does  not  have  access  to  the  ship  measurements  directly  or  the  ship  state  estimate  in  real 
time.  The  data  transmissions  contains  the  delta  change  in  the  ship  information  vector  and  matrix 
since  the  last  transmission  to  the  vehicle  (i.e.  when  the  last  range  measurement  was  sent).  For  the 
example  presented  here,  the  initial  ship  state  at  t o  is  transmitted  to  the  vehicle  at  to  and  received  at 
1 1,  and  the  delta  change  in  the  ship  state  from  to  to  t]  is  transmitted  at  1 1  to  the  vehicle  and  received 
at  t2. 


D.7.1  Ship  Filter 

Because  the  ship  has  no  knowledge  of  the  vehicle  state  or  the  range  measurements  we  compute 
its  trajectory  estimate  first.  To  differentiate  this  filter  from  the  vehicle  filter  we  will  use  y  and  Y  to 
represent  the  information  form  of  the  ship  filter. 

Initial  Conditions  The  ship  starts  at  to  and  we  assume  that 


2/o|o  =  Vs0  (D.102) 

^o|o  =  ASoSo.  (D.103) 
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Prediction  with  Augmentation  t$ t\ 


yi\o 

Y1]0 


0 

Vao 

0  0 

0  ASqSo 

Q71  -Qj'Fs 

—fJ  Qs1  fJ  qs1fs  +  asoso 


Qs1  Q*]Fs 

~ FjQ s1  FjQj'Fs 


Ship  GPS  Measurement  at  t\ 


(D.104) 

(D.105) 

(D.106) 


where 


Vl\z 


=  Vl\0  +  HJpsrgpsZgps{tl) 


0 

Vs0 

Z9i 

Vso 


+ 


*9i 

0 


^i|gpsi  ^  1 1 1  T  HgpsrgpsHgps 


=  ym1  + 


Rg  0 


0 

Qs  1  +  Fg 

-FjQj1 


0 


-QslFs 
FjQj1Fs  +  A 


SQSQ 


(D.107) 


(D.108) 

(D.109) 


(D.110) 


R 


V 


0 

0 


0 

1 

F  rng 


(D.lll) 
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Once  we  are  finished  with  all  of  the  measurements  at  time  step  t\  we  assign 


yi|i  —  yi\zgpsi 

^l|l  =  ^  l|gpsi ■ 


Prediction  with  Augmentation  t\  — > 


0 

A 

2/2 1 1  = 

A 

— 

zgi 

.  y^\ igxi . 

.  Vso  . 

Q71 

- QJ 1 

Y2\i  = 


-fJq;1 

0 


Tn-l  ■ 


F'sQ 


o  ' 

0 

1 - 

o 

o 

0 

+ 

0 

ylll 

0  _ 

0 

Q-1  ~Qs]Fs  0 

- FjQj 1  FjQj'Fs  +  Qj'  +  Rg  -Qj'Fs 

o  ~  fJQs1  fJ  qs1fs  +  asoso 


There  are  no  measurements  to  include  at  time  step  t2  so 


2/2 1 2  —  2/2 1 1 
^2|2  =  ^2|1 


(D.112) 
(D.  113) 


(D.114) 

(D.115) 

(D.116) 


(D.117) 

(D.118) 
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Prediction  with  Augmentation  O  — >  /;> 


2/3 1 2 


% 


0 


0 

0 


Z9l 


Q71 

o 

0 

0  0  0  0 

-FJQ71 

o 

0 

+ 

0 

0 

0  0 

0 

o  y  22 

0 

0  0 

0 

0 

Q71 

-FJQ71 

-Q71Fs 

FjQ^Fs  +  Q 

-1 

s 

0 

-Q7lFs 

0 

-FJQ71 

fJ 

Qs  1FS  +  Qs  1  + 

0 

0 

(D.119) 


(D.120) 


0 

0 


-Qs'Fs 

f]qs1fs  +  asoso 


(D.121) 
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D.7.2  Vehicle  Filter 

For  the  vehicle  filter  we  will  use  the  r]  and  A  variables  for  the  information  filter  vector  and 
matrix  respectively.  The  ship  and  the  vehicle  start  at  to  but  we  assume  that  initially  we  only  have 
knowledge  of  the  vehicle  state. 

Initial  Conditions 


Vo\v0  =  Vv0  (D.122) 

^-o|i>o  =  A-vovo  (D.123) 


Vehicle-Only  Prediction  with  Augmentation  to  — >  t\ 


Vi\v0 


0 

Vv0 

0  0 

0  AVgVo 

Qv1  -QvlFv 

Qv  Fv  Qv  Fv  +  A-vovo 


Q-1  -Q^Fv 
-F^Q-1  FTVQZ1FV 


(D.124) 

(D.125) 

(D.126) 


Incorporate  Initial  Ship  Data  from  Data  Transmission  at  to 

The  ship  information  transmitted  with  the  first  range  measurement  is  just  the  initial  conditions 
of  the  ship: 


V  o|o  =  Vs0  (D.127) 

>0|0  =  A-sqso-  (D.128) 
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We  add  these  new  states  to  the  vehicle  filter: 


rll\vo,SO 


A 


l|uo,so 


^Mv0  2x1 

y^\olxl 

^\V®2xg 

0 


1 

0 

= 

Vv0 

- 

.  Vso  _ 

0 

Y0\0 

lxl  _ 

Qv1 

- FtvQ -1 
0 


-Qv  lFv 
Qv  lFv  +  A 


VO  VO 


0 


0 

0 


A 


■50*0 


After  adding  the  ship  states  let 


Vl\0  Vl^o^o 

Ai|o  —  Ai|„0)S0- 


Range  Measurement  at  t\ 


7 li\zrngi  ?/i|o  T  Hrngrrngzrng(t{) 


0 

%ri 

Vv  0 

+ 

0 

Vs0  _ 

%ri 

Zr  i 
Vv0 

7lso  zr\ 


where 


zri  — 


1 

0 


Zrng(t l) 


'  mg 


(D.129) 

(D.130) 

(D.131) 

(D.132) 

(D.133) 

(D.134) 

(D.135) 

(D.136) 
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A 


l\zr 


^1|0  T  HrngTrngHrng 


Rr 

0 

—Rr 

+ 

o 

<4 

0 

0 

0 

1 

_ 1 

0 

Rr 

'  Qv1 

+  Rr 

F 

V 

—Rr 

-FvQv1 

Fv 

QvlFv 

H-  A  vqvq 

0 

s- 

a? 

i 

_ i 

0 

-^-soso  T  Rr 

where 


Rr 


-4-  0 

F  mg 

0  0 


With  all  of  the  measurements  incorporated  for  this  time  step,  let 


(D.137) 


(D.138) 


(D.139) 


77i|i  -  rh\zr„g1 


At|i™  Ai|i„s 

Ai|is„  Ai|tss 


—  A-i|, 


(D.140) 

(D.141) 
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Vehicle-Only  Prediction  with  Augmentation  t\  — *  O 


V2\vi  ,sq 


0lal 

m\i 


3x1 


Z’, 


r  1 


Vvo 
Vso  ~~ 


1  s> 

o* 

1 _ 

-Q~XFV  0  0  ' 

0 

0  0 

0 

A2|tn,so 

-FyQv1 

0 

Fj,Q-]Fv  0  0 

0  0  0 

+ 

0 

0 

Al|lmi 

Alllus 

0 

0  0  0 

0 

Alll™ 

Alllss 

1  s> 

o* 

1 _ 

-QZlFv 

0 

— Qu 1  Qv  1FV  +  1  +  -Rr 


v  Hi) 

0 


—Rr 


-Q/F 

F~lQ~1Fv  +  A. 


Wo 


(D.142) 


A 


(D.143) 

0 

—  Rr 

0 

So  So  T 

(D.144) 


For  simplicity  of  notation  let 


V2\i  =  V2\Vl,so  (D.145) 

A2|l  =  ^-2|vi,s0-  (D.146) 
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Marginalize  out  Vehicle  State  at  to 

We  marginalize  out  the  vehicle  state  at  to  using  equations  (D.35)  and  (D.36), 


V2\  1  1  ®t»l  )  ) 


0 


Zr  i 

?7so  ~  ^ri 

0 


VV! 

Vsq  ~ri 


0 

-QvlFv 

0 


(Ry  Qv  Fv  A  A„0„0)  7/u0 


-1. 


where 


Vvi  —  zr i  +  Qy  FV(FV  Qv  Fv  A  Ay0y0)  t/Vo 


-l. 


Similarly  for  the  information  matrix, 


■A-2|l(®U2>  ®V1  >  ®So) 


-Qy1^ 


0 


Qy 1 

—  -^rT  Qy  1  Qy  1E’y  A  Qv  1  A  R,  ~  Rr 

^0«0  F  -I^r 


v  -M-v'°6v-M-v''06v  '  ■‘■^r 

0  Rr  AsnSo  +  .R^ 


0 

0 


(F vQv  lFV  A  Aypyp) 


-1 


Tn-i 


o  -RJ  Qs 


Q~v 1 

-RjQy1 

0 


-QvlFv 


o 

—R, 


Rr  ASqSq  H-  R7* 


where 


Ay^  —  F~IQv  1Fv  A  Qv  1  A  Rr  —  Qv  1Fv(F^Qv  1Fv  A  AB0„0)  1F^QV1. 


(D.147) 

(D.148) 

(D.149) 


0 

(D.150) 

(D. 151) 

(D.152) 
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Note:  Unlike  the  centralized  implementation  we  only  have  information  for  the  ship  state  at  to  at  this 
time,  so  we  must  delay  marginalizing  out  the  ship  state  at  t\. 

Vehicle  Velocity  Measurement  at  t,2 


V2\ zvel2  =  V2\l  +  HvelrVelZvel(t2) 


0 
VV! 

7ho  ~~  zr\ 

0 

Vvi 

Vs 0  ~  zr\ 
ZV2 

Vvi 

7ho  ~  zri 


+ 


+ 


0 

1 

0 

0 

ZV2 

0 

0 


Zvelit-  2) 

Tvel 


(D.153) 


(D.154) 


where 


ZV2 


0 

1 


Zvel(t  2) 

^vel 


(D.155) 
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A 


<^‘\zvel2 


A2|i  +  Hjelrv^Hvei 

0 


A-2|l  + 


A-2|l  + 


A-2|l  + 


1 

0 

0 

0 


rvel 


0  — 
rvel 

0 

0 

0  0 
0  0 
0  0 


Rv 

0 

0 


Qv 1  +  R-v  ~Qv  1fv 


V  V 

0 


-Li-ViVi 

—R, 


0  1 


0  0 


0 

0 

0 


0 

—Rr 


A 


SoSo 


+  Rr 


where 


Rr 


0 

0 


0 

l 

rvel  - 


With  all  of  the  measurements  incorporated  for  this  time  step,  let 


(D.156) 


(D.157) 


(D.158) 


V'2\2 

A-2|2 


^2|  Zvel2 


^2\2VV 

A2|2, 

A-2|2S„ 

A2|2. 

A2| 


zvel2 


(D.159) 

(D.160) 
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Vehicle-Only  Prediction  with  Augmentation  £2  — >  0 


0 


^31^2, So 


0 


lxl 


ZV2 

Vvi 


- 1 

N* 

1 

O 

<D 

_ 1 

Qv1 

1 

O 

O 

1  P 

O* 

1 

0  0  0 

0 

A3|d2,so 

-FlQv1 

0 

F~lQ-lFv  0  0 

0  0  0 

+ 

0  . 

0  A2|2„„ 

A-2|2„a 

0 

0  0  0 

0  A2|2s„ 

A2|2ss 

Qv1 

-Qi,lFv 

0 

0 

Fj,  Q,  lFv  +  Qv  1  +  R, 

~Qv 1  Fv 

0 

0 

-FyQv1 

—Rr 

0 

0 

—Rr 

Asgso  T  R 

(D.161) 


(D.162) 


(D.163) 


Incorporate  Delta  Ship  Data  to  to  £2  from  Data  Transmission  at  £2 

The  ship  information  transmitted  with  the  second  range  measurement  is  the  change  in  the  ship 
state  from  to  to  1 2,  taking  into  account  conformability. 


Ay  =  y2|2 


0 

0 

.  yo|o 


0 
0 

Vao 

0 

z9i 

0 


0 

zgi 

Vs0 


(D.164) 


(D.165) 


(D.166) 
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AT'  =  T2|2 


0  0  0 

0  0  0 

o  o  y0|0 


(D.167) 


1  cc 

O* 

1 _ 

-Qj'Fs 

0 

'  0 

0 

0 

-FjQj1 

FjQ-'Fs  +  Q-1  +Rg 

QJ]FS 

- 

0 

0 

0 

0 

-FjQj1 
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_  0 

0 

Asoso  _ 

Q s1 

-FjQj1 

0 


-Q7lFs  o 

Fj  Qj1Fs  +  Q~ 1  +  Rg  QJlFs 

- FjQj 1  FlQ-'Fs  _ 


(D.168) 

(D.169) 


The  delta  ship  states  arc  added  to  the  vehicle  filter,  again  taking  info  account  conformability. 


10  0  0 

0 

0  10  0 
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0  0  10 

0 

V3\v2,S2 
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^31^2, so 
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(D.170) 
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1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 
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1 

0 
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0 
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0 

0 

0 

0 

0 

0 

0 

0 
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0 

0 

0 
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1 

0 

0 
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+ 
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0 
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0 
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0 

0 
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AY 
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0 

(D.171) 


Qv1 

-QvlFv 
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ft 

V 
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JQJ1 
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Qv1 

~QvlFv 

0 

FJ,  Qv1 

ft 

V 

Qv  1FV  +  Qv  1  +  Rv 

-QvlFv 

0 

-FTvQvl 
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0 

0 

0 

0 

0 

0 

0 

—  Rr 

0 

0 

0 

0 

0 

0 

0 

0 

—Rr 

QJ1 

-Q^Fs 

0 

-F 

IQs1 

Fj  Qs  1Fs  +  Qs  1  +  Rg 

-Q^Fs 

0 

-FjQj1 

FjQj1 

Fs  +  Asqso  +  R 

(D.172) 
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Again  for  ease  of  notation,  after  adding  the  ship  states  let 

(D.173) 
(D.174) 


%|2  1 13\V2,S2 

^3|2  =  ^3|«2,S2- 


Marginalize  out  Ship  State  at  t\ 

Now  we  are  able  to  marginalize  out  the  ship  state  at  t\  using  equations  (D.35)  and  (D.36), 


V3\2(XV3 !  XV2 1  xvi j  XS2  > 


0 

0 

ZV2 

0 

Vv  1 

- 

0 

0 

-Qj'Fs 

1 

Co 

O 

1 

■i 

I _ 

.  -FjQj1  _ 

(FlQjiF'  +  Qj'  +  Rgy'zn 


0 

ZV2 

Vv  1 
VS2 
Vs  0 


(D.175) 


(D.176) 


where  fjVl  was  defined  in  equation  (D.149)  and 


Vs2  =  Qj^siFj  Q-lFs  +  QJ1  +  Rg)-lzgi  (D.177) 

Vs0  =  Vs0  -  zri  +  Fj Q~x{f1  Q~xFa  +  Q-1  +  Rg)-lzgi.  (D.178) 
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Similarly  for  the  information  matrix, 


^-3|2(a 


Qv1 

-Q^Fv 

0 

0 

0 

F v  Qv  1  F  J  Qv  lF v  +  Qv  1  +  Rv 

-QvlF 

„  0 

0 

0 

-F^Qv1 

A-V1V1 

0 

—Rr 

0 

0 

0 

Q71 

0 

0 

0 

—Rr 

0 

fJ Qs  1fs  +  ASoSo  +  R 
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O  O  O 
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(FjQ^Fs  +  Qj1 

+  Rg)-1 

1 

O 

O 

0  -FjQj1  -Qj'Fs 

-Qs'Fs 

.  -FjQj1  _ 

Qv1 

-fJ,q~1 
=  0 
0 
0 


1-t  Tp 


-Qv 

Fv  Qv  lFv  +  Qv  1  +  Rv 

-ftvq-1 


0 

0 


0 

-QvlFv 

A. 


0  0 

0  0 

vivi  0  R 

0 

—Rr 


AS2a2  A  S2SQ 


A  s0s2  A  sQs0 


(D.179) 


where  A.VlVl  was  defined  in  equation  (D.  152)  and 


As2S2 

As2«0 

AsoS2 

AsqSO 


QJ1  ~  Q-'F^FjQj'Fs  +  Q-1  +  Rgr'FjQ-1 
-Q-'Fs^FlQ-'Fs  +  Q-1  +  Rgy'Q-'Fs 
-Fj  Q-\Fj  Q;lFs  +  QJ1  +  Rg)~1Fj  Qj1 

FjQ^Fs  +  AS0S0  +  Rr  -  FjQj1(FjQj1Fs  +  Q;1  +  R^Q^Fs 
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Range  Measurement  at  £3 
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where  we  define  zr3 


ztzeM'  Similarly  for  the  information  matrix, 
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where  we  define  Rr 
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Once  all  of  the  measurements  are  incorporated  for  this  time  step,  let 


V3\3=VS\ zrng3  (D.188) 

A3|3  =  A3|*r„93  •  (D.189) 


Marginalize  out  xs(to),  Ship  State  at  to 

We  will  now  marginalize  out  the  ship  state  at  to.  This  will  illustrate  what  happens  when  a  state 
associated  with  a  range  measurement  is  marginalized  out. 

Using  equations  (D.35)  and  (D.36), 
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where 


Vvi  =  fjvi  +  Rr  KosoV  so 

Vs2  =  Vs2  —  "r3  ~~  A-S2SoA-sosQfjs0- 


(D.192) 

(D.193) 
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Similarly  for  the  information  matrix, 
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D.8  Comparison  of  Decentralized  versus  Cen¬ 
tralized  Results 

Comparing  the  centralized  results  for  r/  and  A  at  £3  after  marginalizing  out  the  ship  state,  (D.94) 
and  (D.97),  to  the  decentralized  results,  (D.191)  and  (D.195),  we  see  that  the  vehicle  estimates 
are  identical.  The  ship  estimates  are  not  identical  because  the  decentralized  results  do  not  have 
an  estimate  for  the  current  ship  state,  i.e.  the  ship  state  at  £3,  but  instead  only  have  an  estimate  of 
the  ship  state  up  to  £2,  where  £2  is  the  time  that  the  last  range  measurement  was  sent.  However,  if 
you  compare  the  ship  state  estimate  at  £3  in  the  decentralized  results  before  the  range  measurement 
and  before  the  ship  state  a  £0  is  marginalized  out,  equations  (D.  176)  and  (D.179),  to  the  ship  state 
estimate  at  £2  in  the  centralized  results,  equations  (D.71)  and  (D.74),  as  expected  the  ship  state 
estimates  are  identical.  These  results  indicate  that  for  a  linear  system  with  linear  measurements  we 
are  able  to  locally  recreate  an  exact  replica  of  the  centralized  estimate  of  vehicle  state  at  every  time 
step.  In  addition,  we  are  able  to  locally  recreate  an  exact  replica  of  the  centralized  estimate  of  ship 
state  up  to  the  time  that  the  last  range  measurement  was  sent. 
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